#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <netinet/in.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <errno.h>
#include <net/if_arp.h>

#include "wireless_copy.h"
#include "common.h"
#include "l2_packet.h"
#include "wsc_supplicant.h"
#include "ieee80211_ioctl.h"
#include "eap_defs.h"
#include "eap_wsc.h"
#include "priv_netlink.h"
#include "wscconfig.h"
#include "hostapd_conf.h"
#include "zydas_common.h"
#include <time.h>

int start_time = 0;
struct wpa_driver_zydas_data {
    void *wext; /* private data for driver_wext */
    void *ctx;
    char ifname[IFNAMSIZ + 1];
    int sock;
};
static int zydas_ioctl(struct wsc_drv *drv, struct zydas_wlan_param *param,
             int len, int show_err)
{
    int s;
    struct iwreq iwr;

    s = socket(PF_INET, SOCK_DGRAM, 0);
    if (s < 0) {
        perror("socket");
        return -1;
    }
    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
    iwr.u.data.pointer = (caddr_t) param;
    iwr.u.data.length = len;

    if (ioctl(s, ZD_IOCTL_WPA, &iwr) < 0) {
        int ret;
        close(s);
        ret = errno;
        if (show_err)
            perror("ioctl[ZD_IOCTL_WPA]");
        return ret;
    }
    close(s);

    return 0;
}

char *wps_state_table[] =
{
    "SM_INIT",
    "SM_START",
    "SM_SEND_IDENTITY",
    "SM_START_WPS_NEGOCIATION",
    "SM_SCAN",
    "SM_CONNECT"
};

extern void handlemsg(struct wsc_drv *wsc_drv, char *buf);
extern void InitStateMachine(u8 *own_addr, u8 *ap_addr, u8 method, u8 *pin);
extern struct wsc_drv *g_wsc;

void wsc_new_state(struct wsc_drv *wsc_drv, wps_state s);
int wpa_supplicant_eapol_send(struct wsc_drv *drv, int type, const u8 *buf,
				    size_t len);
int eap_wsc_process(struct wsc_drv *wsc_drv, u8 *reqData, size_t reqDataLen);
int wsc_supplicant_send_eapol_start(struct wsc_drv *drv);

int
zydas_set_param(struct wsc_drv *drv, int op, int arg, int show_err)
{
    struct iwreq iwr;
    int *i, s, ret = 0;

    s = socket(PF_INET, SOCK_DGRAM, 0);
    if (s < 0) {
        perror("socket[PF_INET,SOCK_DGRAM]");
        return -1;
    }

    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
    i = (int *) iwr.u.name;
    *i++ = op;
    *i++ = arg;

    if (ioctl(s, ZD_IOCTL_PARAM, &iwr) < 0) {
        perror("ioctl[ZD_IOCTL_PARAM]");
        ret = -1;
    }
    close(s);

    return ret;
}

int set80211param(struct wsc_drv *wsc_drv, int op, int arg, int show_err)
{
    zydas_set_param(wsc_drv, op, arg, show_err);
    return 0;

#if 0
    struct iwreq iwr;
    
    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, wsc_drv->ifname, IFNAMSIZ);
    iwr.u.mode = op;
    memcpy(iwr.u.name+sizeof(__u32), &arg, sizeof(arg));

    if (ioctl(wsc_drv->ioctl_sock, IEEE80211_IOCTL_SETPARAM, &iwr) < 0) {
        perror("ioctl[IEEE80211_IOCTL_SETPARAM]");
        return -1;
    }
    return 0;
#endif
}

int
set80211priv(struct wsc_drv *wsc_drv, int op, void *data, int len)
{
    struct iwreq iwr;

    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, wsc_drv->ifname, IFNAMSIZ);
    if (len < IFNAMSIZ && op != IEEE80211_IOCTL_FILTERFRAME) {
        /*
         * Argument data fits inline; put it there.
         */
        memcpy(iwr.u.name, data, len);
    } else {
         /*
          * Argument data too big for inline transfer; setup a
          * parameter block instead; the kernel will transfer
          * the data for the driver.
          */
         iwr.u.data.pointer = data;
         iwr.u.data.length = len;
    }

    if (ioctl(wsc_drv->ioctl_sock, op, &iwr) < 0) {
        perror("ioctl");
        return -1;
    }
    return 0;
}

void wsc_printf(int level, char *fmt, ...)
{
	va_list ap;

	va_start(ap, fmt);
	if (g_wsc->debug & level) {
		vprintf(fmt, ap);
	}
	va_end(ap);
}
int wpa_driver_wext_get_range(struct wsc_drv *drv)
{
	struct iw_range *range;
	struct iwreq iwr;
	int minlen;
	size_t buflen;

	/*
	 * Use larger buffer than struct iw_range in order to allow the
	 * structure to grow in the future.
	 */
	buflen = sizeof(struct iw_range) + 500;
	range = malloc(buflen);
	if (range == NULL)
		return -1;
	memset(range, 0, buflen);

	memset(&iwr, 0, sizeof(iwr));
	strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
	iwr.u.data.pointer = (caddr_t) range;
	iwr.u.data.length = buflen;

	minlen = ((char *) &range->enc_capa) - (char *) range +
		sizeof(range->enc_capa);

	if (ioctl(drv->ioctl_sock, SIOCGIWRANGE, &iwr) < 0) {
		perror("ioctl[SIOCGIWRANGE]");
		free(range);
		return -1;
	} else if (iwr.u.data.length >= minlen &&
		   range->we_version_compiled >= 18) {
        wsc_printf(WSC_DBG_CONNECT, "SIOCGIWRANGE: WE(compiled)=%d "
			   "WE(source)=%d enc_capa=0x%x\n",
			   range->we_version_compiled,
			   range->we_version_source,
			   range->enc_capa);
		//drv->has_capability = 1;
		drv->we_version_compiled = range->we_version_compiled;
		drv->we_version_source = range->we_version_source;
		/* no use and no declaration now */
		//if (range->enc_capa & IW_ENC_CAPA_WPA) {
		//	drv->capa.key_mgmt |= WPA_DRIVER_CAPA_KEY_MGMT_WPA |
		//		WPA_DRIVER_CAPA_KEY_MGMT_WPA_PSK;
		//}
		//if (range->enc_capa & IW_ENC_CAPA_WPA2) {
		//	drv->capa.key_mgmt |= WPA_DRIVER_CAPA_KEY_MGMT_WPA2 |
		//		WPA_DRIVER_CAPA_KEY_MGMT_WPA2_PSK;
		//}
		//drv->capa.enc |= WPA_DRIVER_CAPA_ENC_WEP40 |
		//	WPA_DRIVER_CAPA_ENC_WEP104;
		//if (range->enc_capa & IW_ENC_CAPA_CIPHER_TKIP)
		//	drv->capa.enc |= WPA_DRIVER_CAPA_ENC_TKIP;
		//if (range->enc_capa & IW_ENC_CAPA_CIPHER_CCMP)
		//	drv->capa.enc |= WPA_DRIVER_CAPA_ENC_CCMP;
		//wpa_printf(MSG_DEBUG, "  capabilities: key_mgmt 0x%x enc 0x%x",
		//	   drv->capa.key_mgmt, drv->capa.enc);
	} else {
		wsc_printf(WSC_DBG_CONNECT, "SIOCGIWRANGE: too old (short) data - "
			   "assuming WPA is not supported\n");
	}

	free(range);
	return 0;
}


int wpa_driver_wext_get_ifflags(struct wsc_drv *drv, char *ifname, int *flags)
{
    struct ifreq ifr;

    memset(&ifr, 0, sizeof(ifr));
    strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
    if (ioctl(drv->ioctl_sock, SIOCGIFFLAGS, (caddr_t) &ifr) < 0) {
        perror("ioctl[SIOCGIFFLAGS]");
        return -1;
    }
    *flags = ifr.ifr_flags & 0xffff;
    return 0;
}

int wpa_driver_wext_set_ifflags(struct wsc_drv *drv, char *ifname, int flags)
{
    struct ifreq ifr;

    memset(&ifr, 0, sizeof(ifr));
    strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
    ifr.ifr_flags = flags & 0xffff;
    if (ioctl(drv->ioctl_sock, SIOCSIFFLAGS, (caddr_t) &ifr) < 0) {
        perror("ioctl[SIOCSIFFLAGS]");
        return -1;
    }
    return 0;
}

int 
wps_driver_zydas_wps_filter(struct wsc_drv *drv, int enable)
{
    printf("%s : WPS Filter Beacon/ProbeRsp = %d\n",__func__,enable);
    if(zydas_set_param(drv, ZD_PARAM_WPS_FILTER,enable,1))
    {
        printf("set ZD_PARAM_WPS_FILTER failed\n");
        return -1;
    }
    return 0;
}

static int
wpa_driver_zydas_set_wpa_ie(struct wsc_drv * drv,
    const char *wpa_ie, size_t wpa_ie_len)
{


    struct zydas_wlan_param *param;
    int res;
    size_t blen = ZD_GENERIC_ELEMENT_HDR_LEN + wpa_ie_len;
    if (blen < sizeof(*param))
        blen = sizeof(*param);

    param = (struct zydas_wlan_param *) malloc(blen);
    if (param == NULL)
        return -1;

    memset(param, 0, blen);
    param->cmd = ZD_CMD_SET_GENERIC_ELEMENT;
    param->u.generic_elem.len = wpa_ie_len;
    memcpy(param->u.generic_elem.data, wpa_ie, wpa_ie_len);
    res = zydas_ioctl(drv, param, blen, 1);

    free(param);

    return res;
}



int wpa_driver_wext_set_bssid(struct wsc_drv *drv, const u8 *bssid)
{
    struct iwreq iwr;
    int ret = 0;

    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
    iwr.u.ap_addr.sa_family = ARPHRD_ETHER;
    if (bssid)
        memcpy(iwr.u.ap_addr.sa_data, bssid, ETH_ALEN);
    else
        memset(iwr.u.ap_addr.sa_data, 0, ETH_ALEN);

    if (ioctl(drv->ioctl_sock, SIOCSIWAP, &iwr) < 0) {
        perror("ioctl[SIOCSIWAP]");
        ret = -1;
    }

    return ret;
}
#if 0
static int
wpa_driver_madwifi_set_wpa_ie(struct wsc_drv *wsc_drv,
			      const u8 *wpa_ie, size_t wpa_ie_len)
{
    struct iwreq iwr;

    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, wsc_drv->ifname, IFNAMSIZ);
    /* NB: SETOPTIE is not fixed-size so must not be inlined */
    iwr.u.data.pointer = (void *) wpa_ie;
    iwr.u.data.length = wpa_ie_len;

    if (ioctl(wsc_drv->ioctl_sock, IEEE80211_IOCTL_SETOPTIE, &iwr) < 0) {
        perror("ioctl[IEEE80211_IOCTL_SETOPTIE]");
        return -1;
    }
    return 0;
}
#endif

/**
 * wpa_driver_wext_set_ssid - Set SSID, SIOCSIWESSID
 * @priv: Pointer to private wext data from wpa_driver_wext_init()
 * @ssid: SSID
 * @ssid_len: Length of SSID (0..32)
 * Returns: 0 on success, -1 on failure
 */
int wpa_driver_wext_set_ssid(struct wsc_drv *wsc_drv, const u8 *ssid, size_t ssid_len)
{
	struct iwreq iwr;
	int ret = 0;
	char buf[33];

	if (ssid_len > 32)
		return -1;

	memset(&iwr, 0, sizeof(iwr));
	strncpy(iwr.ifr_name, wsc_drv->ifname, IFNAMSIZ);
	/* flags: 1 = ESSID is active, 0 = not (promiscuous) */
	iwr.u.essid.flags = (ssid_len != 0);
	memset(buf, 0, sizeof(buf));
	memcpy(buf, ssid, ssid_len);
	iwr.u.essid.pointer = (caddr_t) buf;
	/* For historic reasons, set SSID length to include one extra
	 * character, C string nul termination, even though SSID is really an
	 * octet string that should not be presented as a C string. Some Linux
	 * drivers decrement the length by one and can thus end up missing the
	 * last octet of the SSID if the length is not incremented here. */
	iwr.u.essid.length = ssid_len ? ssid_len + 1 : 0;

	if (ioctl(wsc_drv->ioctl_sock, SIOCSIWESSID, &iwr) < 0) {
		perror("ioctl[SIOCSIWESSID]");
		ret = -1;
	}

	return ret;
}

#if 0
static int wpa_driver_madwifi_start_receive_beacon(struct wsc_drv *wsc_drv)
{
	struct ieee80211req_set_filter filt;

    wsc_printf(WSC_DBG_INIT, "%s Enter\n", __FUNCTION__);
	filt.app_filterype = IEEE80211_FILTER_TYPE_BEACON |
	                     IEEE80211_FILTER_TYPE_PROBE_RESP;

	return set80211priv(wsc_drv, IEEE80211_IOCTL_FILTERFRAME, &filt,
            			sizeof(struct ieee80211req_set_filter));
}
#endif
int madwifi_driver_associate(struct wsc_drv *drv,
		struct wpa_driver_associate_params *params)
{

    int ret = 0, privacy=1;


    /* Because there might be the case, two or more APs with the same
     * SSID, in order to identify them, we need to set the BSSID. */
    if (wpa_driver_zydas_set_wpa_ie(drv, params->wpa_ie, params->wpa_ie_len) < 0)
        ret = -1;

    if (params->pairwise_suite == CIPHER_NONE &&
        params->group_suite == CIPHER_NONE &&
        params->key_mgmt_suite == KEY_MGMT_NONE &&
        params->wpa_ie_len == 0)
        privacy = 0;

    //if (zydas_set_param(drv, ZD_PARAM_PRIVACY, privacy, 1) < 0)
    //    ret = -1;
    if (zydas_set_param(drv, ZD_PARAM_PRIVACY, params->privacy, 1) < 0)
        ret = -1;

    if (params->wpa_ie_len &&
        zydas_set_param(drv, ZD_PARAM_WPA,
              params->wpa_ie[0] == RSN_INFO_ELEM ? 2 : 1, 1) < 0)
        ret = -1;

    if (params->bssid == NULL) {
        /* ap_scan=0 or 2 mode - driver takes care of AP selection and
         * roaming */
        /* FIX: this does not seem to work; would probably need to
         * change something in the driver */
        //if (zydas_set_param(drv, ZD_PARAM_ROAMING, 0, 1) < 0)
        //    ret = -1;
    //  if (wpa_driver_wext_set_ssid(drv->wext, params->ssid, params->ssid_len) < 0)
    //      ret = -1;
    }

    if (wpa_driver_wext_set_ssid(drv, params->ssid, params->ssid_len) < 0)
        ret = -1;

    // Mark for test
    //if (wpa_driver_wext_set_bssid(ifname, bssid) < 0)
    //  ret = -1;
    if (params->bssid)
    {
        //if (zydas_set_param(drv, ZD_PARAM_ROAMING, 1, 1) < 0)
        //    ret = -1;
        if (wpa_driver_wext_set_bssid(drv, params->bssid) < 0)
            ret = -1;
    }


#if 0
    /* Allow unencrypted EAPOL messages even if pairwise keys are set when
     * not using WPA. IEEE 802.1X specifies that these frames are not
     * encrypted, but WPA encrypts them when pairwise keys are in use. */
    if (key_mgmt_suite == KEY_MGMT_802_1X ||
        key_mgmt_suite == KEY_MGMT_PSK)
        allow_unencrypted_eapol = 0;
    else
        allow_unencrypted_eapol = 1;

    if (prism2param(ifname, PRISM2_PARAM_IEEE_802_1X,
            allow_unencrypted_eapol) < 0) {
        wpa_printf(MSG_DEBUG, "hostap: Failed to configure "
               "ieee_802_1x param");
        /* Ignore this error.. driver_hostap.c can also be used with
         * other drivers that do not support this prism2_param. */
    }
#endif
    return ret;

}
#if 0
int madwifi_driver_associate(struct wsc_drv *wsc_drv,
		struct wpa_driver_associate_params *params)
{
    struct ieee80211req_mlme mlme;
    int ret = 0, privacy = 1;

    /*
     * NB: Don't need to set the freq or cipher-related state as
     *     this is implied by the bssid which is used to locate
     *     the scanned node state which holds it.  The ssid is
     *     needed to disambiguate an AP that broadcasts multiple
     *     ssid's but uses the same bssid.
     */
    /* XXX error handling is wrong but unclear what to do... */
    if (wpa_driver_madwifi_set_wpa_ie(wsc_drv, params->wpa_ie,
                   params->wpa_ie_len) < 0) {
        ret = -1;
    }

    if (params->pairwise_suite == CIPHER_NONE &&
        params->group_suite == CIPHER_NONE &&
        params->key_mgmt_suite == KEY_MGMT_NONE &&
        params->wpa_ie_len == 0) {
            privacy = 0;
    }

    if (set80211param(wsc_drv, IEEE80211_PARAM_PRIVACY, privacy, 1) < 0) {
        ret = -1;
    }

    if (params->wpa_ie_len &&
        set80211param(wsc_drv, IEEE80211_PARAM_WPA,
        params->wpa_ie[0] == RSN_INFO_ELEM ? 2 : 1, 1) < 0) {
           ret = -1;
    }

    if (params->bssid == NULL) {
        /* ap_scan=2 mode - driver takes care of AP selection and
         * roaming */
        /* FIX: this does not seem to work; would probably need to
         * change something in the driver */
         if (set80211param(wsc_drv, IEEE80211_PARAM_ROAMING, 0, 1) < 0)
             ret = -1;

         if (wpa_driver_wext_set_ssid(wsc_drv, params->ssid,
             params->ssid_len) < 0) {
                ret = -1;
         }
    }
    else {
        if (set80211param(wsc_drv, IEEE80211_PARAM_ROAMING, 2, 1) < 0)
            ret = -1;

        if (wpa_driver_wext_set_ssid(wsc_drv, params->ssid,
            params->ssid_len) < 0) {
                ret = -1;
        }

        memset(&mlme, 0, sizeof(mlme));
        mlme.im_op = IEEE80211_MLME_ASSOC;
        memcpy(mlme.im_macaddr, params->bssid, IEEE80211_ADDR_LEN);
        if (set80211priv(wsc_drv, IEEE80211_IOCTL_SETMLME, &mlme,
                sizeof(mlme)) < 0)
        {
            wsc_printf(WSC_DBG_CONNECT, "%s: SETMLME failed\n", __FUNCTION__);
            ret = -1;
        }
    }

#if 0
    if (params->bssid) {
        if (set80211param(drv, IEEE80211_PARAM_ROAMING, 2, 1) < 0)
	    ret = -1;
        memset(&mlme, 0, sizeof(mlme));
        mlme.im_op = IEEE80211_MLME_ASSOC;
        memcpy(mlme.im_macaddr, params->bssid, IEEE80211_ADDR_LEN);
        if (set80211priv(drv, IEEE80211_IOCTL_SETMLME, &mlme,
            sizeof(mlme), 1) < 0)
            ret = -1;
    }
#endif

    return ret;
}
#endif
int wsc_supplicant_deauthenticate(struct wsc_drv *drv, u8 *addr,
    int reason_code)
{
    struct zydas_wlan_param *param;
    int blen;
        int ret;

    blen = sizeof(*param);
    param = (struct zydas_wlan_param *) malloc(blen);
    if (param == NULL)
        return -1;

    memset(param, 0, blen);
    param->cmd = ZD_CMD_SET_MLME;
    param->u.mlme.cmd = MLME_STA_DEAUTH;
    param->u.mlme.reason_code = reason_code;
    memcpy(param->sta_addr, addr, ETH_ALEN);
    ret = zydas_ioctl(drv, param, blen, 1);
        usleep(100000);
        free(param);
        return ret;

#if 0
    struct ieee80211req_mlme mlme;

    mlme.im_op = IEEE80211_MLME_DEAUTH;
    mlme.im_reason = reason_code;
    memcpy(mlme.im_macaddr, addr, IEEE80211_ADDR_LEN);
    return set80211priv(drv, IEEE80211_IOCTL_SETMLME, &mlme, sizeof(mlme));
#endif
}

int wsc_set_timeout(struct wsc_drv *wsc_drv, int secs, void *user_data)
{
    struct eloop_timeout *timeout, *tmp, *prev;

    printf("Enter Timeout : %s (%d sec),%d\n",(char *)user_data,secs,__LINE__);
    timeout = (struct eloop_timeout *) malloc(sizeof(*timeout));
    if (timeout == NULL)
    {
        printf("Set Timeout Failed @ %s,%d\n",__FILE__,__LINE__);
        return -1;
    }
    gettimeofday(&timeout->time, NULL);
    timeout->time.tv_sec += secs;
    //timeout->time.tv_usec += usecs;
    //while (timeout->time.tv_usec >= 1000000) {
    //    timeout->time.tv_sec++;
    //    timeout->time.tv_usec -= 1000000;
    //}
    //timeout->eloop_data = eloop_data;
    timeout->user_data = user_data;
    //timeout->handler = handler;
    timeout->next = NULL;

    if (wsc_drv->timeout == NULL) {
        wsc_drv->timeout = timeout;
        return 0;
    }

    prev = NULL;
    tmp = wsc_drv->timeout;
    while (tmp != NULL) {
        if (timercmp(&timeout->time, &tmp->time, <))
            break;
        prev = tmp;
        tmp = tmp->next;
    }

    if (prev == NULL) {
        timeout->next = wsc_drv->timeout;
        wsc_drv->timeout = timeout;
    } else {
        timeout->next = prev->next;
        prev->next = timeout;
    }

    return 0;
}

int wsc_supplicant_cancel_timeout(struct wsc_drv *wsc_drv, void *user_data)
{
    struct eloop_timeout *timeout, *prev, *next;
    int removed = 0;

    printf("Cancel Timeout : %s", (char *)user_data);
    prev = NULL;
    timeout = wsc_drv->timeout;
    while (timeout != NULL) {
        next = timeout->next;

        if (timeout->user_data == user_data) {
            if (prev == NULL)
                wsc_drv->timeout = next;
            else
                prev->next = next;
            free(timeout);
            removed++;
        }
        else
            prev = timeout;

        timeout = next;
    }
    printf(" (count=%d)\n", removed);

    return removed;
}

int hex2num(char c)
{
    if (c >= '0' && c <= '9')
        return c - '0';
    if (c >= 'a' && c <= 'f')
        return c - 'a' + 10;
    if (c >= 'A' && c <= 'F')
        return c - 'A' + 10;
    return -1;
}

int hex2byte(const char *hex)
{
    int a,b;
    a = hex2num(*hex++);
    if (a < 0)
        return -1;
    b = hex2num(*hex++);
    if (b < 0)
        return -1;
    return (a << 4) | b;
}

int hexstr2bin(const char *hex, u8 *buf, size_t len)
{
    size_t i;
    int a;
    const char *ipos = hex;
    u8 *opos = buf;

    for (i = 0; i < len; i++) {
        a = hex2byte(ipos);
        if (a < 0)
            return -1;
        *opos++ = a;
        ipos += 2;
    }
    return 0;
}

static void *madwifi_driver_init(void *ctx, const char *ifname)
{
    struct wsc_drv *drv = (struct wsc_drv *)ctx;
    int flags;

    wsc_printf(WSC_DBG_INIT, "Entering %s\n", __FUNCTION__);

    drv->ioctl_sock = socket(PF_INET, SOCK_DGRAM, 0);

    /* Set the interface wds_ap up
    */
    if (drv->wds_ap_ifname != NULL)
    {
        if (wpa_driver_wext_get_ifflags(drv, drv->wds_ap_ifname, &flags) != 0 ||
            wpa_driver_wext_set_ifflags(drv, drv->wds_ap_ifname, flags | IFF_UP) != 0) {
            wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", drv->wds_ap_ifname);
        }
    }

    /* Set the interface wds_sta up
    */
    if (wpa_driver_wext_get_ifflags(drv, drv->ifname, &flags) != 0 ||
        wpa_driver_wext_set_ifflags(drv, drv->ifname, flags | IFF_UP) != 0) {
        wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", drv->ifname);
    }

    /* Set the interface up */
    if (wpa_driver_wext_get_ifflags(drv, drv->ifname, &flags) != 0 ||
        wpa_driver_wext_set_ifflags(drv, drv->ifname, flags | IFF_UP) != 0) {
        wsc_printf(WSC_DBG_INIT, "Could not set interface %s UP\n", drv->ifname);
    }

    /*if (set80211param(drv, IEEE80211_PARAM_CWM_MODE, IEEE80211_CWM_MODE20, 1) < 0) {
        wsc_printf(WSC_DBG_INIT, "%s: failed to set cwm mode\n", __FUNCTION__);
            goto fail;
    }*/

#if 0
    if (set80211param(drv, IEEE80211_PARAM_ROAMING, 2, 1) < 0) {
        wsc_printf(WSC_DBG_INIT, "%s: failed to set wpa_supplicant-based "
                "roaming\n", __FUNCTION__);
            goto fail;
    }

    if (set80211param(drv, IEEE80211_PARAM_WPA, 3, 1) < 0) {
        wsc_printf(WSC_DBG_INIT, "%s: failed to enable WPA support\n",
                __FUNCTION__);
            goto fail;
    }
#endif

    //if (wpa_driver_madwifi_start_receive_beacon(drv) < 0)
    if(wps_driver_zydas_wps_filter(drv,1))
    {
        wsc_printf(WSC_DBG_INIT, "%s: failed to set receiving management frames\n",
                __FUNCTION__);
    }
    wpa_driver_wext_get_range(drv);
    return drv;

//fail:
    close(drv->ioctl_sock);
    return NULL;
}

u16 wsc_find_atheros_oui(struct wsc_drv *wsc_drv, const u8 *buf, u16 len)
{
    u16 offset, ii;
    char oui[] = {0x0, 0x3, 0x7f};

    offset = 36; /* first element offset */

    /* Search loop */
    while (offset < len)
    {
        /* Search target element */
        if (buf[offset] == 0xDD)
        {
            /* Bingo */
            if (buf[offset + 1] > (len - offset))
            {
                /* Element length error */
                return 0xffff;
            }

            if ((buf[offset + 2] == 0x00)
                    && (buf[offset + 3] == 0x50)
                    && (buf[offset + 4] == 0xF2)
                    && (buf[offset + 5] == 0x04))

            {
                /* find atheros oui */
                for (ii = 6;ii < (buf[offset + 1] - 3); ii++)
                {
                    if (memcmp((char *)&buf[offset + ii], oui, 3) == 0)
                    {
                        return (offset + ii + 3);
                    }
                }
            }
        }
        /* Advance to next element */
        if (buf[offset + 1] == 0)
        {
            return 0xffff;
        }
        offset += (buf[offset + 1] + 2);
    }
    return 0xffff;
}

u16 wsc_find_wps_ie(struct wsc_drv *wsc_drv, const u8 *buf, u16 len, int *isPBC)
{
    u16 offset, ii;
    u16 policy_0_offset;
    char pbc_pattern[] = {0x10, 0x12, 0x00, 0x02, 0x00, 0x04, 0x10, 0x53, 0x00, 0x02, 0x00};
    char pbc_pattern2[] = {0x10, 0x12, 0x00, 0x02, 0x00, 0x04, 0x10, 0x53, 0x00, 0x02, 0x01};
    char pin_pattern[] = {0x10, 0x12, 0x00, 0x02, 0x00, 0x00, 0x10, 0x53, 0x00, 0x02, 0x00};

    offset = 36; /* first element offset */

    /* Search loop */
    *isPBC = 2; // not WPS
    while (offset < len)
    {
        /* Search target element */
        if (buf[offset] == 0xDD)
        {
            /* Bingo */
            if (buf[offset + 1] > (len - offset))
            {
                /* Element length error */
                return 0xffff;
            }

            if ((buf[offset + 2] == 0x00)
                    && (buf[offset + 3] == 0x50)
                    && (buf[offset + 4] == 0xF2)
                    && (buf[offset + 5] == 0x04))

            {
                policy_0_offset = offset;
                *isPBC = 3; // WPS without known pattern
                
                /* find pbc or pin pattern */
                for (ii = 6;ii < (buf[offset + 1] - (sizeof(pbc_pattern) - 3)); ii++)
                {
                    if (wsc_drv->policy == 0)
                    {
                            if (memcmp((char *)&buf[offset + ii], pbc_pattern, sizeof(pbc_pattern)) == 0)
                            {
                                *isPBC = 1;
                                return offset + ii;
                            }
                            if (memcmp((char *)&buf[offset + ii], pbc_pattern2, sizeof(pbc_pattern2)) == 0)
                            {
                                *isPBC = 1;
                                return offset + ii;
                            }
                            if (memcmp((char *)&buf[offset + ii], pin_pattern, sizeof(pin_pattern)) == 0)
                            {
                                *isPBC = 0;
                                return offset + ii;
                            }
                    }
                    else
                    {
                        if (wsc_drv->pin == NULL)
                        {
                            if (memcmp((char *)&buf[offset + ii], pbc_pattern, sizeof(pbc_pattern)) == 0)
                            {
                                *isPBC = 1;
                                return offset + ii;
                            }
                            if (memcmp((char *)&buf[offset + ii], pbc_pattern2, sizeof(pbc_pattern2)) == 0)
                            {
                                *isPBC = 1;
                                return offset + ii;
                            }

                        }
                        else
                        {
                            if (memcmp((char *)&buf[offset + ii], pin_pattern, sizeof(pin_pattern)) == 0)
                            {
                                *isPBC = 0;
                                return offset + ii;
                            }
                        }
                    }
                }
                if (wsc_drv->policy == 0)
                    return policy_0_offset;
            }
        }
        /* Advance to next element */
        if (buf[offset + 1] == 0)
        {
            return 0xffff;
        }
        offset += (buf[offset + 1] + 2);
    }
    return 0xffff;
}


u16 wsc_get_essid(struct wsc_drv *wsc_drv, const u8 *buf, u8 *essidbuf, u16 len)
{
    u16 offset, i;

    offset = 36; /* first element offset */

    /* Search loop */
    while (offset < len)
    {
        /* Search target element */
        if (buf[offset] == 0x0)
        {
            /* Bingo */
            if (buf[offset + 1] > (len - offset))
            {
                /* Element length error */
                return 0xffff;
            }

            for (i = 0; i < buf[offset + 1] + 2; i++)
            {
                essidbuf[i] = buf[offset + i];
            }

            return offset;
        }
        /* Advance to next element */
        if (buf[offset + 1] == 0)
        {
            return 0xffff;
        }
        offset += (buf[offset + 1] + 2);
    }
    return 0xffff;
}

const u8 *wpa_drv_get_mac_addr(struct wsc_drv *wsc_drv)
{
    return NULL;
}

u8 wsc_check_block_list(struct wsc_drv *wsc_drv, u8 *bssid)
{
    u8 ii;
    int i;
    
    for (ii = 0; ii < wsc_drv->blockAPNum; ii++)
    {
        if (memcmp(wsc_drv->blockBssid[ii], bssid, ETH_ALEN) == 0)
        {
            for(i=0;i<6;i++)
                printf("%02x ",bssid[i] & 0xFF);
            printf("Blocked,time=%d\n",wsc_drv->blockTime[ii]);

            return 1;;
        }
    }
    return 0;
}

void wsc_decrease_and_remove_block_ap(struct wsc_drv *wsc_drv)
{
    u8 ii, j;
    
    for (ii = 0; ii < wsc_drv->blockAPNum; ii++)
    {
        wsc_drv->blockTime[ii]--;
    }

    ii = 0;
    while (ii < wsc_drv->blockAPNum)
    {
        if (wsc_drv->blockTime[ii] == 0)
        {
            if (ii <= (wsc_drv->blockAPNum - 1))
            {
                for (j = ii + 1; j < wsc_drv->blockAPNum; j++)
                {
                    memcpy(wsc_drv->blockBssid[j - 1], wsc_drv->blockBssid[j], ETH_ALEN);
                }
            }
            wsc_drv->blockAPNum--;
        }
        else
        {
            ii++;
        }
    }
}

void wsc_supplicant_rx(void *ctx, const u8 *src_addr, const u8 *buf, size_t len)
{
    int matched = 0;
    int i, isPBC = -1; //0 = PIN, 1 = PBC, 2 = NOT WPS, 3 = WPS without known pattern

    int ii, wpsPos = 0;//, ouiPos = 0;
    char essidbuf[34], bssid[6];
    struct wsc_drv *wsc_drv = (struct wsc_drv *) ctx;
    struct ieee802_1x_hdr *hdr = (struct ieee802_1x_hdr *) buf;
    struct eap_hdr *eap_hdr = (struct eap_hdr *) (hdr+1);
    struct eap_hdr *rsp = (struct eap_hdr *)wsc_drv->rspbuf;
    char *p;

    wsc_printf(WSC_DBG_MSGWSC, "wsc_supplicant_rx enter, len: %d\n", len);
    if(len < sizeof(struct ieee802_1x_hdr))
    {
	    printf("Invalid eap frame\n");
        return;
    }


    for (ii = 0; ii < len; ii++)
    {
        wsc_printf(WSC_DBG_MSGWSC, "0x%02x ", buf[ii]);
        if ((ii & 0x0f) == 0x0f)
            wsc_printf(WSC_DBG_MSGWSC, "\n");
    }

    wsc_printf(WSC_DBG_MSGWSC, "\n");

    switch(wsc_drv->state)
    {
    case SM_SCAN:
        /* Get WPS IE Position */
        wpsPos = wsc_find_wps_ie(wsc_drv, buf, len, &isPBC);
        if (wpsPos != 0xffff)
        {
#if 0
            /* Get Atheros OUI Position */
            ouiPos = wsc_find_atheros_oui(wsc_drv, buf, len);
            if (ouiPos != 0xffff)
            {
                /* WPS IE with Atheros OUI but service type of STA is public */
                if (wsc_drv->serviceType == 1)
                    break;
                /* Atheros OUI is front of WPS IE */
                if (wpsPos < ouiPos)
                    break;
                /* WPS IE with Atheros OUI but service type of STA is not match with service type of AP */
                if (wsc_drv->serviceType != buf[ouiPos])
                    break;
            }
            else
#endif
            {
                /* service type of AP is public but service type of STA is not */
                if (wsc_drv->serviceType != 1)
                    break;
            }
            memcpy(bssid, &buf[10], ETH_ALEN);
            for(i=0;i<wsc_drv->wpsAPNum;i++)
              if(memcmp(wsc_drv->wpsBSSID[i], bssid, 6) == 0)
              {
                matched = 1;
                break;
              }
            if(!matched)
            {
                memcpy(wsc_drv->wpsBSSID[wsc_drv->wpsAPNum], bssid,6);
                wsc_drv->wpsAPNum++;
                if(isPBC == 1)
                    wsc_drv->pbcActivates++;
            }

            //wsc_printf(WSC_DBG_CONNECT,
            //    "found WPS IE in Rx management frames, mac is %02x:%02x:%02x:%02x:%02x:%02x\n",
            //    bssid[0], bssid[1], bssid[2], bssid[3], bssid[4], bssid[5]);
            
            if (wsc_get_essid(wsc_drv, buf, essidbuf, len) != 0xffff)
            {
                if((wsc_drv->policy == 0) && (wsc_drv->pin == NULL) && (isPBC==1))
                {
                    for (ii = 0; ii < essidbuf[1]; ii++)
                        wsc_drv->PBCssidBody[ii] = essidbuf[2 + ii];
                    wsc_drv->PBCssidBody[ii] = '\0';
                    wsc_drv->PBCssid = wsc_drv->PBCssidBody;
                    wsc_drv->PBCssid_len = strlen(wsc_drv->PBCssidBody);
                    printf("policy=0, SSID=%s\n", wsc_drv->PBCssidBody);
                }
	            if (wsc_check_block_list(wsc_drv, bssid))
	            {
	                break;
	            }

                if ((wsc_drv->ssid_len != 0) && (wsc_drv->pbcAPNum != 0))
                {
                    if (wsc_drv->policy == 0)
                        break;
                        
                    if (memcmp(wsc_drv->ssidBody, (char *)&essidbuf[2], wsc_drv->ssid_len) != 0)
                    {
                        wsc_drv->ssid_len = 0;
                        wsc_drv->pbcAPNum++;
                        break;
                    }
                }
                else
                {
                    wsc_drv->pbcAPNum++;
                    for (ii = 0; ii < essidbuf[1]; ii++)
                        wsc_drv->ssidBody[ii] = essidbuf[2 + ii];
                    wsc_drv->ssidBody[ii] = '\0';
                    wsc_drv->ssid = wsc_drv->ssidBody;
                    wsc_drv->ssid_len = strlen(wsc_drv->ssidBody);
                    //printf("CWY - ssid: %s\n", wsc_drv->ssidBody);
                    //printf("CWY - ssidlen: %d\n", wsc_drv->ssid_len);
                }
            }
        }
        break;
    case SM_START:
        if (hdr->type != IEEE802_1X_TYPE_EAP_PACKET)
        {
            wsc_printf(WSC_DBG_MSGWSC, "State: SM_START, Receive a frame which is not EAP packet\n");
            break;
        }

        if (eap_hdr->code != EAP_CODE_REQUEST)
        {
            wsc_printf(WSC_DBG_MSGWSC, "State: SM_START, Not a EAP_REQUEST packet\n");
            break;
        }

        // TODO: Check whether this is a hello packet

        rsp->code = EAP_CODE_RESPONSE;
        rsp->identifier = eap_hdr->identifier;
        rsp->length = htons(1 + strlen(WFA_IDENTITY) + sizeof(struct eap_hdr));
        p = (char *)(rsp + 1);
        *p++ = EAP_TYPE_IDENTITY;
        strncpy(p, WFA_IDENTITY, sizeof(WFA_IDENTITY));

        // Cancel timeout
        wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

        /* Send a eap response frame */
        wpa_supplicant_eapol_send(wsc_drv, IEEE802_1X_TYPE_EAP_PACKET, (u8 *)rsp,
                      ntohs(rsp->length));

        wsc_new_state(wsc_drv, SM_SEND_IDENTITY);
        
        wsc_drv->resendTime = 0;

        wsc_set_timeout(wsc_drv, WSC_TIMEOUT_INIT, wps_state_table[wsc_drv->state]);
        break;

    case SM_SEND_IDENTITY:
    case SM_START_WPS_NEGOCIATION:
        /* Cancel the connect timer */
        wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

        wsc_drv->resendTime = 3;
        
        /* Set timeout */
        wsc_set_timeout(wsc_drv, 5/*WSC_TIMEOUT_DEFAULT*/, wps_state_table[wsc_drv->state]);

        eap_wsc_process(wsc_drv, (u8 *)eap_hdr, len-sizeof(*hdr));
        break;
    case SM_CONNECT:
    case SM_INIT:
        break;
    }

}

void wsc_kill_process(pid_t pid)
{
    int pidStatus;

    printf("kill process, pid = %d\n", pid);

    kill(pid, 15);
    
    waitpid(pid, &pidStatus, 0);
}

/**
 * wpa_driver_wext_get_bssid - Get BSSID, SIOCGIWAP
 * @priv: Pointer to private wext data from wpa_driver_wext_init()
 * @bssid: Buffer for BSSID
 * Returns: 0 on success, -1 on failure
 */
int wpa_driver_wext_get_bssid(struct wsc_drv *drv, u8 *bssid)
{
    struct iwreq iwr;
    int ret = 0;

    memset(&iwr, 0, sizeof(iwr));
    strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);

    if (ioctl(drv->ioctl_sock, SIOCGIWAP, &iwr) < 0) {
        perror("ioctl[SIOCGIWAP]");
        ret = -1;
    }
    memcpy(bssid, iwr.u.ap_addr.sa_data, ETH_ALEN);

    return ret;
}

/**
 * wpa_driver_wext_get_ssid - Get SSID, SIOCGIWESSID
 * @priv: Pointer to private wext data from wpa_driver_wext_init()
 * @ssid: Buffer for the SSID; must be at least 32 bytes long
 * Returns: SSID length on success, -1 on failure
 */
int wpa_driver_wext_get_ssid(struct wsc_drv *drv, u8 *ssid)
{
	struct iwreq iwr;
	int ret = 0;

	memset(&iwr, 0, sizeof(iwr));
	strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
	iwr.u.essid.pointer = (caddr_t) ssid;
	iwr.u.essid.length = 32;

	if (ioctl(drv->ioctl_sock, SIOCGIWESSID, &iwr) < 0) {
		perror("ioctl[SIOCGIWESSID]");
		ret = -1;
	} else
		ret = iwr.u.essid.length;

	return ret;
}

/**
 * wpa_driver_wext_scan - Request the driver to initiate scan
 * Returns: 0 on success, -1 on failure
 */
int wpa_driver_wext_scan(struct wsc_drv *drv)
{
	struct iwreq iwr;
	int ret = 0;

	memset(&iwr, 0, sizeof(iwr));
	strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);

    //wsc_printf(WSC_DBG_CONNECT, "CWY - enter wpa_driver_wext_scan and ioctl[SIOCSIWSCAN] to driver\n");
	if (ioctl(drv->ioctl_sock, SIOCSIWSCAN, &iwr) < 0) {
		perror("ioctl[SIOCSIWSCAN]");
		ret = -1;
	}
    //wsc_printf(WSC_DBG_CONNECT, "CWY - ioctl[SIOCSIWSCAN] to driver end\n");

	/* Not all drivers generate "scan completed" wireless event, so try to
	 * read results after a timeout. */
        wsc_set_timeout(drv, WSC_TIMEOUT_SCAN, wps_state_table[drv->state]);

	return ret;
}

/**
 * wpa_driver_wext_get_scan_results - Fetch the latest scan results
 * @priv: Pointer to private wext data from wpa_driver_wext_init()
 * @results: Pointer to buffer for scan results
 * @max_size: Maximum number of entries (buffer size)
 * Returns: Number of scan result entries used on success, -1 on
 * failure
 *
 * If scan results include more than max_size BSSes, max_size will be
 * returned and the remaining entries will not be included in the
 * buffer.
 */
int wpa_driver_wext_get_scan_results(struct wsc_drv *drv,
				     struct wpa_scan_result *results,
				     size_t max_size)
{
	struct iwreq iwr;
	size_t ap_num = 0;
	int first, maxrate;
	u8 *res_buf;
	struct iw_event iwe_buf, *iwe = &iwe_buf;
	char *pos, *end, *custom, *genie, *gpos, *gend;
	struct iw_param p;
	size_t len, clen, res_buf_len;
    int sleepTime = 0;

	memset(results, 0, max_size * sizeof(struct wpa_scan_result));

	res_buf_len = IW_SCAN_MAX_DATA;
	for (;;) {
		res_buf = malloc(res_buf_len);
		if (res_buf == NULL)
        {
            printf("out of memory. Please revise driver to allocate smaller buffer\n");
			return -ENOMEM;
        }
again:
		memset(&iwr, 0, sizeof(iwr));
		strncpy(iwr.ifr_name, drv->ifname, IFNAMSIZ);
		iwr.u.data.pointer = res_buf;
		iwr.u.data.length = res_buf_len;

		if (ioctl(drv->ioctl_sock, SIOCGIWSCAN, &iwr) == 0)
			break;

		if (errno == E2BIG && res_buf_len < 100000) {
			free(res_buf);
			res_buf = NULL;
			res_buf_len *= 2;
			wsc_printf(WSC_DBG_CONNECT, "Scan results did not fit - "
				   "trying larger buffer (%lu bytes)\n",
				   (unsigned long) res_buf_len);
		} 
        else if(errno == EAGAIN && (sleepTime < 10))
        {
            sleepTime++;
            sleep(1);
            printf("Sleep %d time\n", sleepTime);
            goto again;
        }
        else 
        {
			perror("ioctl[SIOCGIWSCAN]");
			free(res_buf);
			return -1;
		}
	}

	len = iwr.u.data.length;
	ap_num = 0;
	first = 1;

	pos = (char *) res_buf;
	end = (char *) res_buf + len;

	while (pos + IW_EV_LCP_LEN <= end) {
		int ssid_len;
		/* Event data may be unaligned, so make a local, aligned copy
		 * before processing. */
		memcpy(&iwe_buf, pos, IW_EV_LCP_LEN);
		if (iwe->len <= IW_EV_LCP_LEN)
			break;

		custom = pos + IW_EV_POINT_LEN;
		if ((drv->we_version_compiled > 18) &&
		    (iwe->cmd == SIOCGIWESSID ||
		     iwe->cmd == SIOCGIWENCODE ||
		     iwe->cmd == IWEVGENIE ||
		     iwe->cmd == IWEVCUSTOM)) {
			/* WE-19 removed the pointer from struct iw_point */
			char *dpos = (char *) &iwe_buf.u.data.length;
			int dlen = dpos - (char *) &iwe_buf;
			memcpy(dpos, pos + IW_EV_LCP_LEN,
			       sizeof(struct iw_event) - dlen);
		} else 
		{
			memcpy(&iwe_buf, pos, sizeof(struct iw_event));
			custom += IW_EV_POINT_OFF;
		}

		switch (iwe->cmd) {
		case SIOCGIWAP:
			if (!first)
				ap_num++;
			first = 0;
			if (ap_num < max_size) {
				memcpy(results[ap_num].bssid,
				       iwe->u.ap_addr.sa_data, ETH_ALEN);
			}
			break;
		case SIOCGIWMODE:
			if (ap_num >= max_size)
				break;
			if (iwe->u.mode == IW_MODE_ADHOC)
				results[ap_num].caps |= IEEE80211_CAP_IBSS;
			else if (iwe->u.mode == IW_MODE_MASTER ||
				 iwe->u.mode == IW_MODE_INFRA)
				results[ap_num].caps |= IEEE80211_CAP_ESS;
			break;
		case SIOCGIWESSID:
			ssid_len = iwe->u.essid.length;
			if (custom + ssid_len > end)
				break;
			if (iwe->u.essid.flags &&
			    ssid_len > 0 &&
			    ssid_len <= IW_ESSID_MAX_SIZE) {
				if (ap_num < max_size) {
					memcpy(results[ap_num].ssid, custom,
					       ssid_len);
					results[ap_num].ssid_len = ssid_len;
				}
			}
			break;
		case SIOCGIWFREQ:
			if (ap_num < max_size) {
				int divi = 1000000, i;
				if (iwe->u.freq.e > 6) {
					wsc_printf(WSC_DBG_CONNECT,
						"Invalid freq "
						"in scan results (BSSID="
						MACSTR ": m=%d e=%d\n",
						MAC2STR(results[ap_num].bssid),
						iwe->u.freq.m, iwe->u.freq.e);
					break;
				}
				for (i = 0; i < iwe->u.freq.e; i++)
					divi /= 10;
				results[ap_num].freq = iwe->u.freq.m / divi;
			}
			break;
		case IWEVQUAL:
			if (ap_num < max_size) {
				results[ap_num].qual = iwe->u.qual.qual;
				results[ap_num].noise = iwe->u.qual.noise;
				results[ap_num].level = iwe->u.qual.level;
			}
			break;
		case SIOCGIWENCODE:
			if (ap_num < max_size &&
			    !(iwe->u.data.flags & IW_ENCODE_DISABLED))
				results[ap_num].caps |= IEEE80211_CAP_PRIVACY;
			break;
		case SIOCGIWRATE:
			custom = pos + IW_EV_LCP_LEN;
			clen = iwe->len;
			if (custom + clen > end)
				break;
			maxrate = 0;
			while (((ssize_t) clen) >=
			       (ssize_t) sizeof(struct iw_param)) {
				/* Note: may be misaligned, make a local,
				 * aligned copy */
				memcpy(&p, custom, sizeof(struct iw_param));
				if (p.value > maxrate)
					maxrate = p.value;
				clen -= sizeof(struct iw_param);
				custom += sizeof(struct iw_param);
			}
			if (ap_num < max_size)
				results[ap_num].maxrate = maxrate;
			break;
		case IWEVGENIE:
			if (ap_num >= max_size)
				break;
			gpos = genie = custom;
			gend = genie + iwe->u.data.length;
			if (gend > end) {
				wsc_printf(WSC_DBG_CONNECT, "IWEVGENIE overflow\n");
				break;
			}
			while (gpos + 1 < gend &&
			       gpos + 2 + (u8) gpos[1] <= gend) {
				u8 ie = gpos[0], ielen = gpos[1] + 2;
				if (ielen > SSID_MAX_WPA_IE_LEN) {
					gpos += ielen;
					continue;
				}
				switch (ie) {
				case GENERIC_INFO_ELEM:
					if (ielen < 2 + 4 ||
					    memcmp(&gpos[2],
						   "\x00\x50\xf2\x01", 4) != 0)
						break;
					memcpy(results[ap_num].wpa_ie, gpos,
					       ielen);
					results[ap_num].wpa_ie_len = ielen;
					break;
				case RSN_INFO_ELEM:
					memcpy(results[ap_num].rsn_ie, gpos,
					       ielen);
					results[ap_num].rsn_ie_len = ielen;
					break;
				}
				gpos += ielen;
			}
			break;
		case IWEVCUSTOM:
			clen = iwe->u.data.length;
			if (custom + clen > end)
				break;
			if (clen > 7 && strncmp(custom, "wpa_ie=", 7) == 0 &&
			    ap_num < max_size) {
				char *spos;
				int bytes;
				spos = custom + 7;
				bytes = custom + clen - spos;
				if (bytes & 1)
					break;
				bytes /= 2;
				if (bytes > SSID_MAX_WPA_IE_LEN) {
					wsc_printf(WSC_DBG_CONNECT, "Too long WPA IE "
						   "(%d)\n", bytes);
					break;
				}
				hexstr2bin(spos, results[ap_num].wpa_ie,
					   bytes);
				results[ap_num].wpa_ie_len = bytes;
			} else if (clen > 7 &&
				   strncmp(custom, "rsn_ie=", 7) == 0 &&
				   ap_num < max_size) {
				char *spos;
				int bytes;
				spos = custom + 7;
				bytes = custom + clen - spos;
				if (bytes & 1)
					break;
				bytes /= 2;
				if (bytes > SSID_MAX_WPA_IE_LEN) {
					wsc_printf(WSC_DBG_CONNECT, "Too long RSN IE "
						   "(%d)\n", bytes);
					break;
				}
				hexstr2bin(spos, results[ap_num].rsn_ie,
					   bytes);
				results[ap_num].rsn_ie_len = bytes;
			}
			break;
		}

		pos += iwe->len;
	}
	free(res_buf);
	res_buf = NULL;
	if (!first)
		ap_num++;
	if (ap_num > max_size) {
		wsc_printf(WSC_DBG_CONNECT, "Too small scan result buffer - "
			   "%lu BSSes but room only for %lu\n",
			   (unsigned long) ap_num,
			   (unsigned long) max_size);
		ap_num = max_size;
	}
	//qsort(results, ap_num, sizeof(struct wpa_scan_result),
	//      wpa_scan_result_compar);

	wsc_printf(WSC_DBG_CONNECT, "Received %lu bytes of scan results (%lu BSSes)\n",
		   (unsigned long) len, (unsigned long) ap_num);

	return ap_num;
}

void wpa_driver_wext_event_wireless(struct wsc_drv *wsc_drv,
        char *data, int len)
{
	struct iw_event iwe_buf, *iwe = &iwe_buf;
	char *pos, *end, *custom;
    int flags;

	pos = data;
	end = data + len;

	while (pos + IW_EV_LCP_LEN <= end) {
		/* Event data may be unaligned, so make a local, aligned copy
		 * before processing. */
		memcpy(&iwe_buf, pos, IW_EV_LCP_LEN);
		wsc_printf(WSC_DBG_CONNECT, "Wireless event: cmd=0x%x len=%d\n",
			   iwe->cmd, iwe->len);
		if (iwe->len <= IW_EV_LCP_LEN)
			return;

		custom = pos + IW_EV_POINT_LEN;
		//if (drv->we_version_compiled > 18 &&
#if 0
		 if (iwe->cmd == IWEVMICHAELMICFAILURE ||
		     iwe->cmd == IWEVCUSTOM ||
		     iwe->cmd == IWEVASSOCREQIE ||
		     iwe->cmd == IWEVASSOCRESPIE ||
		     iwe->cmd == IWEVPMKIDCAND) {
			/* WE-19 removed the pointer from struct iw_point */
			char *dpos = (char *) &iwe_buf.u.data.length;
			int dlen = dpos - (char *) &iwe_buf;
			memcpy(dpos, pos + IW_EV_LCP_LEN,
			       sizeof(struct iw_event) - dlen);
		} 
		else 
#endif
		{
			memcpy(&iwe_buf, pos, sizeof(struct iw_event));
			custom += IW_EV_POINT_OFF;
		}

		switch (iwe->cmd) {
		case SIOCGIWAP:
			printf("Wireless event: new AP: "
				   MACSTR,
				   MAC2STR((u8 *) iwe->u.ap_addr.sa_data));

            if (wsc_drv->pushButtonPress == 0)
                break;

			if (memcmp(iwe->u.ap_addr.sa_data,
				   "\x00\x00\x00\x00\x00\x00", ETH_ALEN) == 0
			    ||
			    memcmp(iwe->u.ap_addr.sa_data,
				   "\x44\x44\x44\x44\x44\x44", ETH_ALEN) == 0)
			{
			//	free(drv->assoc_req_ies);
			//	drv->assoc_req_ies = NULL;
			//	free(drv->assoc_resp_ies);
			//	drv->assoc_resp_ies = NULL;
			//	wpa_supplicant_event(ctx, EVENT_DISASSOC,
			//			     NULL);
			//
				//if (wsc_drv->state == SM_SEND_IDENTITY)
				//    break;

				/* If we already associated with an AP, deauthenticate */
				if (memcmp(wsc_drv->bssid, "\x00\x00\x00\x00\x00\x00",
					ETH_ALEN) != 0)
				{
					wsc_supplicant_deauthenticate(wsc_drv, wsc_drv->bssid, 3);

					/* Cancel timeout */
					wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

					wsc_new_state(wsc_drv, SM_SCAN);
					wsc_set_timeout(wsc_drv, 1/*WSC_TIMEOUT_INIT*/, wps_state_table[wsc_drv->state]);
                                }

				memcpy(wsc_drv->bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN);
				wsc_printf(WSC_DBG_CONNECT, "AP address are all zeros\n");
			}
			else
			{
				if (wsc_drv->state == SM_CONNECT)
				{
					/* Cancel timeout */
					wsc_drv->connect_try = 0;
					wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

                    //wsc_drv->state = SM_START;
					wsc_new_state(wsc_drv, SM_START);
					
					wsc_supplicant_send_eapol_start(wsc_drv);

					memcpy(wsc_drv->bssid, iwe->u.ap_addr.sa_data, ETH_ALEN);

                    /* Initialize state machine */
                    if (wsc_drv->pin == NULL)
                        InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x04, NULL);
                    else
                        InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x0, wsc_drv->pin);

					wsc_printf(WSC_DBG_CONNECT, "Own_addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
						wsc_drv->l2->own_addr[0], wsc_drv->l2->own_addr[1],
						wsc_drv->l2->own_addr[2], wsc_drv->l2->own_addr[3],
						wsc_drv->l2->own_addr[4], wsc_drv->l2->own_addr[5]);

                    wsc_printf(WSC_DBG_CONNECT, "AP addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
						wsc_drv->bssid[0], wsc_drv->bssid[1],
						wsc_drv->bssid[2], wsc_drv->bssid[3],
						wsc_drv->bssid[4], wsc_drv->bssid[5]);

                    wsc_set_timeout(wsc_drv, 3/*WSC_TIMEOUT_DEFAULT*/, wps_state_table[wsc_drv->state]);
				}
			}
			break;
	//	case IWEVMICHAELMICFAILURE:
	//		wpa_driver_wext_event_wireless_michaelmicfailure(
	//			drv, ctx, custom, iwe->u.data.length);
	//		break;
		case IWEVCUSTOM:
			if (custom + iwe->u.data.length > end)
				break;
			if (strncmp(custom+4, "PUSH-BUTTON.indication", 22) == 0){
				wsc_printf(WSC_DBG_CONNECT, "PUSH-BUTTON.indication event occurs\n");
				/* kill wpa_supplicant process */
				if (wsc_drv->wpa_supplicant_pid != 0)
				{
                    wsc_kill_process(wsc_drv->wpa_supplicant_pid);
                    wsc_kill_process(wsc_drv->wpa_supplicant_pid + 2);
                    system( "killall hostapd" ); /* kill hostapd because of wsccmd would not do it */
                    sleep(3); /* wait 2 seconds, hostapd eloop bug */
                    wsc_drv->wpa_supplicant_pid = 0;
                }

                #if 0
				/* kill hostapd process, wsccmd should do this */
                if (wsc_drv->hostapd_pid != 0)
				{
                    wsc_kill_process(wsc_drv->hostapd_pid);
                    wsc_drv->hostapd_pid = 0;
                }
                #endif

                if (wsc_drv->wds_ap_ifname != NULL)
                {
			    	/* kill wsccmd process */
                    if (wsc_drv->wsccmd_pid != 0)
	    			{
                        wsc_kill_process(wsc_drv->wsccmd_pid);
                        wsc_drv->wsccmd_pid = 0;
                    }
                }

                /* 
                 * Moved to beginning of madwifi_init(). This brings up the
                 * AP and STA vaps before any ioctl()s are set.
                 */ 
#if 1
                /* Set the interface wds_ap up
                */
                if (wsc_drv->wds_ap_ifname != NULL)
                {
                    if (wpa_driver_wext_get_ifflags(wsc_drv, wsc_drv->wds_ap_ifname, &flags) != 0 ||
                        wpa_driver_wext_set_ifflags(wsc_drv, wsc_drv->wds_ap_ifname, flags | IFF_UP) != 0) {
                        wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", wsc_drv->wds_ap_ifname);
                    }
                }

                /* Set the interface wds_sta up
                */
                if (wpa_driver_wext_get_ifflags(wsc_drv, wsc_drv->ifname, &flags) != 0 ||
                    wpa_driver_wext_set_ifflags(wsc_drv, wsc_drv->ifname, flags | IFF_UP) != 0) {
                    wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", wsc_drv->ifname);
                }
#endif
#ifdef L2DEINIT
                if (wsc_drv->l2 != NULL)
                {
                    l2_packet_deinit(wsc_drv->l2);
                    wsc_drv->l2 = NULL;
                }
                
                wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                                        ETH_P_ALL/*ETH_P_EAPOL*/, wsc_supplicant_rx, wsc_drv, 0);
#else
     	        wps_driver_zydas_wps_filter(wsc_drv,1);
#endif
                
                /* Check whether we need to initialize bridge socket */
                if (wsc_drv->bridge_ifname != NULL)
                {
                    wsc_printf(WSC_DBG_INIT, "Initialize bridge interface\n");
                
                    if (wsc_drv->l2_br == NULL)
                    {
                        wsc_drv->l2_br = l2_packet_init(wsc_drv->bridge_ifname,
                                                    NULL,
                                                    ETH_P_EAPOL, wsc_supplicant_rx,
                                                    wsc_drv, 0);
                    }
                }

				/* Cancel timeout */
				wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

				wsc_new_state(wsc_drv, SM_SCAN);

				memcpy(wsc_drv->bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN);
				wsc_printf(WSC_DBG_CONNECT, "Set AP address are all zeros\n");

                wsc_drv->ssid_len = 0;
                
                wsc_drv->pbcAPNum = 0;
                
                wsc_drv->pushButtonPress = 1;

                /* Set timeout */
                wsc_set_timeout(wsc_drv, WSC_TIMEOUT_SCAN, wps_state_table[wsc_drv->state]);
			}
	//		if (custom + iwe->u.data.length > end)
	//			return;
	//		buf = malloc(iwe->u.data.length + 1);
	//		if (buf == NULL)
	//			return;
	//		memcpy(buf, custom, iwe->u.data.length);
	//		buf[iwe->u.data.length] = '\0';
	//		wpa_driver_wext_event_wireless_custom(drv, ctx, buf);
	//		free(buf);
			break;
	//	case SIOCGIWSCAN:
	//		eloop_cancel_timeout(wpa_driver_wext_scan_timeout,
	//				     drv, ctx);
	//		wpa_supplicant_event(ctx, EVENT_SCAN_RESULTS, NULL);
	//		break;
	//	case IWEVASSOCREQIE:
	//		wpa_driver_wext_event_wireless_assocreqie(
	//			drv, ctx, custom, iwe->u.data.length);
	//		break;
	//	case IWEVASSOCRESPIE:
	//		wpa_driver_wext_event_wireless_assocrespie(
	//			drv, ctx, custom, iwe->u.data.length);
	//		break;
	//	case IWEVPMKIDCAND:
	//		wpa_driver_wext_event_wireless_pmkidcand(
	//			drv, ctx, custom, iwe->u.data.length);
	//		break;
		}

		pos += iwe->len;
	}
}

static void wpa_driver_wext_event_rtm_newlink(struct wsc_drv *drv,
        struct nlmsghdr *h, int len)
{
    struct ifinfomsg *ifi;
    int attrlen, nlmsg_len, rta_len;
    struct rtattr * attr;

    if (len < sizeof(*ifi))
        return;

    ifi = NLMSG_DATA(h);

    if (drv->ifindex != ifi->ifi_index)
    {
        wsc_printf(WSC_DBG_CONNECT, "Ignore event for foreign ifindex %d\n",
            ifi->ifi_index);
        return;
    }

    nlmsg_len = NLMSG_ALIGN(sizeof(struct ifinfomsg));

    attrlen = h->nlmsg_len - nlmsg_len;
    if (attrlen < 0)
        return;

    attr = (struct rtattr *) (((char *) ifi) + nlmsg_len);

    rta_len = RTA_ALIGN(sizeof(struct rtattr));
    while (RTA_OK(attr, attrlen)) {
        if (attr->rta_type == IFLA_WIRELESS) {
            wpa_driver_wext_event_wireless(
                    drv, ((char *) attr) + rta_len,
                    attr->rta_len - rta_len);
         }
         //else if (attr->rta_type == IFLA_IFNAME) {
         //	wpa_driver_wext_event_link(drv, ctx,
         //				   ((char *) attr) + rta_len,
         //				   attr->rta_len - rta_len, 0);
         //}
         attr = RTA_NEXT(attr, attrlen);
    }
}

int wpa_driver_wext_event_receive(struct wsc_drv *wsc_drv)
{
	char buf[8192];
	int left;
	struct sockaddr_nl from;
	socklen_t fromlen;
	struct nlmsghdr *h;

	fromlen = sizeof(from);
	left = recvfrom(wsc_drv->event_sock, buf, sizeof(buf), MSG_DONTWAIT,
			(struct sockaddr *) &from, &fromlen);
	if (left < 0) {
		if (errno != EINTR && errno != EAGAIN)
			perror("recvfrom(netlink)");
		return -1;
	}

	h = (struct nlmsghdr *) buf;
	while (left >= sizeof(*h)) {
		int len, plen;

		len = h->nlmsg_len;
		plen = len - sizeof(*h);
		if (len > left || plen < 0) {
			wsc_printf(WSC_DBG_CONNECT, "Malformed netlink message: "
				   "len=%d left=%d plen=%d\n",
				   len, left, plen);
			break;
		}

		switch (h->nlmsg_type) {
		case RTM_NEWLINK:
			wpa_driver_wext_event_rtm_newlink(wsc_drv,
							  h, plen);
			break;
		}

		len = NLMSG_ALIGN(len);
		left -= len;
		h = (struct nlmsghdr *) ((char *) h + len);
	}

	if (left > 0) {
		wsc_printf(WSC_DBG_CONNECT, "%d extra bytes in the end of netlink "
			   "message\n", left);
	}

	return 0;
}

static u8 * wpa_alloc_eapol(struct wsc_drv *wpa_s, u8 type,
			    const void *data, u16 data_len,
			    size_t *msg_len, void **data_pos)
{
    struct ieee802_1x_hdr *hdr;

    *msg_len = sizeof(*hdr) + data_len;
    hdr = malloc(*msg_len);
    if (hdr == NULL)
        return NULL;

    hdr->version = DEFAULT_EAPOL_VERSION;
    hdr->type = type;
    hdr->length = htons(data_len);

    if (data)
        memcpy(hdr + 1, data, data_len);
    else
        memset(hdr + 1, 0, data_len);

    if (data_pos)
        *data_pos = hdr + 1;

    return (u8 *) hdr;
}

int wpa_supplicant_eapol_send(struct wsc_drv *drv, int type, const u8 *buf,
				    size_t len)
{
    u8 *msg, *dst, bssid[ETH_ALEN];
    size_t msglen;
    int res;

    msg = dst = NULL;

    memset(bssid,0,6);
    wpa_driver_wext_get_bssid(drv, bssid);
    memcpy(drv->bssid,bssid,6);
    if (memcmp(drv->bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN) == 0) {
        //wpa_printf(MSG_DEBUG, "BSSID not set when trying to send an "
        //	   "EAPOL frame");
        if (wpa_driver_wext_get_bssid(drv, bssid) == 0 &&
            memcmp(bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN) != 0) {
                dst = bssid;
		//	wpa_printf(MSG_DEBUG, "Using current BSSID " MACSTR
		//		   " from the driver as the EAPOL destination",
		//		   MAC2STR(dst));
                memcpy(drv->bssid, bssid, ETH_ALEN);
        }
        else
        {
            printf("BSSID IS STILL ZERO~~~~\n");
            return -1;
        }

    }
    else {
        /* BSSID was already set (from (Re)Assoc event, so use it as
         * the EAPOL destination. */
        dst = drv->bssid;
    }

    msg = wpa_alloc_eapol(drv, type, buf, len, &msglen, NULL);
    if (msg == NULL)
        return -1;

    // add by ygwei for debug
    //if (drv->l2_br != NULL)
    //    res = l2_packet_send(drv->l2_br, dst, ETH_P_EAPOL, msg, msglen);
    //else
        res = l2_packet_send(drv->l2, dst, ETH_P_EAPOL, msg, msglen);
    free(msg);
    return res;
}

int eap_wsc_process(struct wsc_drv *wsc_drv, u8 *reqData, size_t reqDataLen)
{
    struct eap_hdr *req;
    u8 *sendBuf;
    u32 sendBufLen;
    WSC_NOTIFY_DATA notifyData;
    WSC_NOTIFY_DATA *pWSCData;
    char *pWSCEAP;

    wsc_printf(WSC_DBG_MSGWSC, "@#*@#*@#*EAP-WSC: Entered eap_wsc_process *#@*#@*#@\n");

    req = (struct eap_hdr *) reqData;
    wsc_printf(WSC_DBG_MSGWSC, "EAP-WSC : Received packet(len=%lu)\n", (unsigned long) reqDataLen);

    if(ntohs(req->length) != reqDataLen)
    {
        wsc_printf(WSC_DBG_MSGWSC, "EAP-WSC: Pkt length in pkt(%d) differs from" 
			" supplied (%d)\n", ntohs(req->length), reqDataLen);
	return -1;
    }

    notifyData.type = WSC_NOTIFY_TYPE_PROCESS_REQ;
    notifyData.length = reqDataLen;
    //notifyData.u.process.state = data->state;

    sendBuf = (u8 *) malloc(sizeof(WSC_NOTIFY_DATA) + reqDataLen);
    if ( ! sendBuf)
    {
        wsc_printf(WSC_DBG_MSGWSC, "EAP-WSC: Memory allocation for the sendBuf failed\n");
        return -1;
    }

    memcpy(sendBuf, &notifyData, sizeof(WSC_NOTIFY_DATA));
    memcpy(sendBuf + sizeof(WSC_NOTIFY_DATA), reqData, reqDataLen);
    sendBufLen = sizeof(WSC_NOTIFY_DATA) + reqDataLen;

    pWSCData = (WSC_NOTIFY_DATA *)sendBuf;

    /* Pass the data to Enrolee state machine */
    pWSCEAP = (char *)&(sendBuf[sizeof(*pWSCData)]);
    handlemsg(wsc_drv, pWSCEAP);

    free(sendBuf);

    return 0;
}

void wsc_new_state(struct wsc_drv *wsc_drv, wps_state s)
{
    wsc_printf(WSC_DBG_MSGWSC, "Change from state: %s to state: %s\n",
        wps_state_table[wsc_drv->state], wps_state_table[s]);

    if(s == SM_SCAN)
    {
        wsc_drv->wpsAPNum = 0;
        wsc_drv->pbcActivates = 0;
    }

    wsc_drv->state = s;
}

int wsc_send_udp_data(struct wsc_drv *wsc_drv, char *buf, int len)
{
    //int sendBytes = 0;
    char sendBuf[WSC_RECVBUF_SIZE];
    WSC_NOTIFY_DATA *notifyData;
    u8  wscNotifyCode;
    //WSC_EAP_HEADER *wscEapHdr;
    //struct eap_hdr *req;
    //int recvBytes;
    u8 *resp;
    //u8 *sendBuf;
    //u32 sendBufLen;
    //char recvBuf[WSC_RECVBUF_SIZE];
    //struct sockaddr_in from;
    //struct sockaddr_in to;
    //WSC_NOTIFY_DATA notifyData;
    WSC_NOTIFY_DATA * recvNotify;

    wsc_printf(WSC_DBG_MSGWSC, "In CInbEap::SendDataDown buffer Length = %d\n", len);

    if ((buf == NULL) || (len == 0))
    {
        wsc_printf(WSC_DBG_MSGWSC, "Invalid Parameters\n");
        return -1;
    }

    /* TODO: Set the wscNotifyCode */
    // Fix the NotifyCode
    wscNotifyCode = WSC_NOTIFY_TYPE_PROCESS_RESULT;

    // attach header and send it down
    notifyData = (WSC_NOTIFY_DATA *) sendBuf;
    notifyData->type = wscNotifyCode;

    switch (wscNotifyCode)
    {
        case WSC_NOTIFY_TYPE_BUILDREQ_RESULT:
            if (buf != NULL) {
                notifyData->u.bldReqResult.result = WSC_NOTIFY_RESULT_SUCCESS;
            }
            else {
                notifyData->u.bldReqResult.result = WSC_NOTIFY_RESULT_FAILURE;
            }
            break;
        case WSC_NOTIFY_TYPE_PROCESS_RESULT:
            notifyData->u.processResult.done = 0;
            if (buf != NULL) {
                notifyData->u.processResult.result = WSC_NOTIFY_RESULT_SUCCESS;
            }
            else {
            //    notifyData->u.processResult.result = m_gotReq ? 
            //        WSC_NOTIFY_RESULT_SUCCESS : WSC_NOTIFY_RESULT_FAILURE;
                notifyData->u.processResult.result = WSC_NOTIFY_RESULT_SUCCESS;
            }
            break;
        default:
            break;
    }

    notifyData->length = len;
    memcpy(notifyData + 1, buf, len);

#if 0
    notifyData->length = dataLen + sizeof(WSC_EAP_HEADER);

    wscEapHdr = (WSC_EAP_HEADER *) (notifyData + 1);

    wscEapHdr->code = eapCode;
    wscEapHdr->id = m_recentId;
    wscEapHdr->length = WscHtons(sizeof(WSC_EAP_HEADER) + dataLen);

    wscEapHdr->type = WSC_EAP_TYPE;
    wscEapHdr->vendorId[0] = WSC_VENDORID1;
    wscEapHdr->vendorId[1] = WSC_VENDORID2;
    wscEapHdr->vendorId[2] = WSC_VENDORID3;
    wscEapHdr->vendorType = WscHtonl(WSC_VENDORTYPE);

    if (dataBuffer)
    {
        if (dataBuffer[WSC_MSGTYPE_OFFSET] >= WSC_ID_MESSAGE_M1 &&
                dataBuffer[WSC_MSGTYPE_OFFSET] <= WSC_ID_MESSAGE_M8)
        {
            wscEapHdr->opcode = WSC_MSG;
        }
        else if (dataBuffer[WSC_MSGTYPE_OFFSET] == WSC_ID_MESSAGE_ACK)
        {
            wscEapHdr->opcode = WSC_ACK;
        }
        else if (dataBuffer[WSC_MSGTYPE_OFFSET] == WSC_ID_MESSAGE_NACK)
        {
            wscEapHdr->opcode = WSC_NACK;
        }
        else if (dataBuffer[WSC_MSGTYPE_OFFSET] == WSC_ID_MESSAGE_DONE)
        {
            wscEapHdr->opcode = WSC_Done;
        }
        else
        {
            TUTRACE((TUTRACE_ERR, "Unknown Message Type code %d; "
                "Not sending msg\n", dataBuffer[WSC_MSGTYPE_OFFSET]));
            return TREAP_ERR_SENDRECV;
        }
        // TBD: Flags are always set to zero for now, if message is too big
        // fragmentation must be done here and flags will have some bits set
        // and message length field is added
        wscEapHdr->flags = 0;

        memcpy(wscEapHdr + 1, dataBuffer,  dataLen);
    }
#endif

    recvNotify = (WSC_NOTIFY_DATA *) sendBuf;
    if ( (recvNotify->type != WSC_NOTIFY_TYPE_PROCESS_RESULT) ||
    //     (recvNotify->length == 0) ||
    (recvNotify->u.processResult.result != WSC_NOTIFY_RESULT_SUCCESS) )
    {
        wsc_printf(WSC_DBG_MSGWSC, "EAP-WSC: Process Message failed somewhere\n");
        return -1;
    }
	
    resp = (u8 *) malloc(recvNotify->length);
    if (!resp)
    {
        wsc_printf(WSC_DBG_MSGWSC, "EAP-WSC: Memory allocation for the resp failed\n");
        return -1;
    }

    memcpy(resp, recvNotify + 1, recvNotify->length);
    wpa_supplicant_eapol_send(wsc_drv, IEEE802_1X_TYPE_EAP_PACKET, resp,
                  recvNotify->length);
    free(resp);
    
    return 0;
}

int wsc_register_read_sock(int sock, struct l2_packet_data *l2)
{
    int sock_num = g_wsc->sock_num;

    if (sock_num > MAX_SOCK_NUM)
    {
        wsc_printf(WSC_DBG_INIT, "Exceed maximum sock number\n");
        return -1;
    }

    wsc_printf(WSC_DBG_INIT, "%s: add socket: %d\n", __FUNCTION__, sock);

    g_wsc->sock[sock_num] = sock;
    g_wsc->l2_sock[sock_num] = l2;
    g_wsc->sock_num++;

    if (sock > g_wsc->max_sock_num)
        g_wsc->max_sock_num = sock;

    return 0;
}

int wsc_unregister_read_sock(int sock, struct l2_packet_data *l2)
{
    int sock_num = g_wsc->sock_num;
    int i, j;

    for (i = 0; i < sock_num; i++)
    {
        if ((g_wsc->sock[i] == sock) && (g_wsc->l2_sock[i] == l2))
            break;
    }
    if (i >= sock_num)
    {
        wsc_printf(WSC_DBG_CONNECT, "sock index error\n");
        return 0;
    }
    
    if (i == (sock_num - 1))
    {
        g_wsc->sock[i] = 0;
        g_wsc->l2_sock[i] = NULL;
        g_wsc->sock_num--;
    }
    else
    {
        for (j = i; j < (sock_num - 1); j++)
        {
            g_wsc->sock[j] = g_wsc->sock[j + 1];
            g_wsc->l2_sock[j] = g_wsc->l2_sock[j + 1];
        }
        g_wsc->sock[j] = 0;
        g_wsc->l2_sock[j] = NULL;
        g_wsc->sock_num--;
    }
    
    return 0;
}

int wsc_supplicant_init(struct wsc_drv *wsc_drv)
{
    int s;
    struct sockaddr_nl local;

    madwifi_driver_init(wsc_drv, wsc_drv->ifname);

    /* Initialize layer 2 socket */
#ifdef L2DEINIT
    wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                            ETH_P_EAPOL, wsc_supplicant_rx, wsc_drv, 0);
#else
    wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                            ETH_P_ALL, wsc_supplicant_rx, wsc_drv, 0);
#endif

    if (wsc_drv->l2 == NULL)
    {
        return WSC_ERROR_INIT_SOCKET;
    }

    /* Check whether we need to initialize bridge socket */
    if (wsc_drv->bridge_ifname != NULL)
    {
        wsc_printf(WSC_DBG_INIT, "Initialize bridge interface\n");

        wsc_drv->l2_br = l2_packet_init(wsc_drv->bridge_ifname,
                                        NULL,
                                        ETH_P_EAPOL, wsc_supplicant_rx,
                                        wsc_drv, 0);

        if (wsc_drv->l2_br == NULL)
        {
            wsc_printf(WSC_DBG_INIT, "Can't initialize bridge socket interface\n");
            return WSC_ERROR_INIT_SOCKET;
        }
    }

    /* Initialize netlink socket */
    s = socket(PF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
    if (s < 0) {
        perror("socket(PF_NETLINK,SOCK_RAW,NETLINK_ROUTE)");
        //close(drv->ioctl_sock);
        return WSC_ERROR_INIT_SOCKET;
    }

    memset(&local, 0, sizeof(local));
    local.nl_family = AF_NETLINK;
    local.nl_groups = RTMGRP_LINK;
    if (bind(s, (struct sockaddr *) &local, sizeof(local)) < 0) {
        perror("bind(netlink)");
         //close(s);
         //close(drv->ioctl_sock);
         return WSC_ERROR_INIT_SOCKET;
    }

    wsc_drv->ifindex = if_nametoindex(wsc_drv->ifname);

    //eloop_register_read_sock(s, wpa_driver_wext_event_receive, drv, ctx);
    wsc_register_read_sock(s, NULL);
    wsc_drv->event_sock = s;

    //madwifi_driver_init(wsc_drv, wsc_drv->ifname);

    /* Initialize state machine */
    wsc_drv->state = SM_INIT;

    return 0;
}

int wsc_supplicant_connect(struct wsc_drv *wsc_drv)
{
#define SCAN_AP_LIMIT 64

    int ret;
    struct wpa_driver_associate_params *params;
    struct wpa_scan_result *results;
    int num;
    int bss_idx = SCAN_AP_LIMIT;
    int ii;

    /* TODO */
    /* Issue a site survey command */
    results = malloc(SCAN_AP_LIMIT * sizeof(struct wpa_scan_result));
    if (results == NULL) {
        wsc_printf(WSC_DBG_CONNECT, "Failed to allocate memory for scan results\n");
            return -1;
    }

    num = wpa_driver_wext_get_scan_results(wsc_drv, results, SCAN_AP_LIMIT);
    wsc_printf(WSC_DBG_CONNECT, "Scan results: %d\n", num);

    if (num < 0) {
        wsc_printf(WSC_DBG_CONNECT, "Failed to get scan results\n");
        free(results);
        return -1;
    }
    if (num > SCAN_AP_LIMIT) {
        wsc_printf(WSC_DBG_CONNECT, "Not enough room for all APs (%d < %d)\n",
            num, SCAN_AP_LIMIT);
            num = SCAN_AP_LIMIT;

        free(results);
        return -1;
    }

    for (ii = 0; ii < num; ii++)
    {
        if (memcmp(results[ii].ssid, wsc_drv->ssid, wsc_drv->ssid_len) == 0)
        {
            char *ssid = malloc(wsc_drv->ssid_len+1);

            memcpy(ssid, wsc_drv->ssid, wsc_drv->ssid_len);
            ssid[wsc_drv->ssid_len]=0;
            wsc_drv->ssid[wsc_drv->ssid_len] = '\0';
            memcpy(wsc_drv->connect2BSSID,results[ii].bssid,6);

            wsc_printf(WSC_DBG_CONNECT, "Get the desired SSID: %s\n", ssid);
            bss_idx = ii;
            free(ssid);
            break;
        }
    }

    if (bss_idx == SCAN_AP_LIMIT)
    {
        wsc_printf(WSC_DBG_CONNECT, "Can't find desired SSID: %s\n", wsc_drv->ssid);
        free(results);

        /* Cancel the connect timer */
        wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

        /* Issue a scan command */
        wpa_driver_wext_scan(wsc_drv);

        return -1;
    }

    params = calloc(sizeof(*params), 1);

    /* Fill the parameters */
    params->ssid = wsc_drv->ssid;
    params->ssid_len = wsc_drv->ssid_len;
    params->wpa_ie = NULL;
    params->wpa_ie_len = 0;
    params->bssid = results[bss_idx].bssid;
    params->pairwise_suite = CIPHER_CCMP;
    params->group_suite = CIPHER_CCMP;
    params->key_mgmt_suite = KEY_MGMT_PSK;
    params->privacy = results[bss_idx].caps & IEEE80211_CAP_PRIVACY;


    ret = madwifi_driver_associate(wsc_drv, params);
    printf("madwifi_driver_associate ret = %d\n",ret);
    free(params);
    free(results);

    return ret;
}

int wsc_copy_fd_table(struct wsc_drv *wsc_drv, fd_set *rfds)
{
    int ii;

    for (ii = 0; ii < wsc_drv->sock_num; ii++)
        FD_SET(wsc_drv->sock[ii], rfds);

    return 0;
}

void wsc_supplicant_timeout_handler(struct wsc_drv *wsc_drv)
{
    struct eap_hdr *rsp = (struct eap_hdr *)wsc_drv->rspbuf;
    unsigned char zero[6]={0};
    unsigned char bssid[6]={0};
    unsigned char ssid[33]={0};


    if (wsc_drv->pushButtonPress == 0)
        return;

    wsc_printf(WSC_DBG_MSGWSC, "@@@@@@ %s entering @@@@@\n", __FUNCTION__);
    //wsc_printf(WSC_DBG_CONNECT, "%s entering, state is %s\n",
    //        __FUNCTION__, wps_state_table[wsc_drv->state]);

    if (wsc_drv->state == SM_CONNECT)
    {
        if(!wpa_driver_wext_get_bssid(wsc_drv, bssid))
        {
            if(wpa_driver_wext_get_ssid(wsc_drv, ssid))
            {
                if((memcmp(bssid,zero,6) != 0) &&
                        (ssid[0] != 0)
                  )
                {
                    wsc_drv->connect_try = 0;
                    wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);
                    wsc_new_state(wsc_drv, SM_START);
                    wsc_supplicant_send_eapol_start(wsc_drv);
                    memcpy(wsc_drv->bssid, bssid,6);

                    /* Initialize state machine */
                    if (wsc_drv->pin == NULL)
                        InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x04, NULL);
                    else
                        InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x0, wsc_drv->pin);

                    printf("A Join Event Lost\n");
                    wsc_printf(WSC_DBG_CONNECT, "Own_addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
                            wsc_drv->l2->own_addr[0], wsc_drv->l2->own_addr[1],
                            wsc_drv->l2->own_addr[2], wsc_drv->l2->own_addr[3],
                            wsc_drv->l2->own_addr[4], wsc_drv->l2->own_addr[5]);

                    wsc_printf(WSC_DBG_CONNECT, "AP addr: %02x:%02x:%02x:%02x:%02x:%02x\n",
                            wsc_drv->bssid[0], wsc_drv->bssid[1],
                            wsc_drv->bssid[2], wsc_drv->bssid[3],
                            wsc_drv->bssid[4], wsc_drv->bssid[5]);

                    wsc_set_timeout(wsc_drv, 5/*WSC_TIMEOUT_DEFAULT*/, wps_state_table[wsc_drv->state]);
                    return;
                }
            }
        }

        /* Cancel the connect timer */
        wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

        /* Do STA connect */
        if(wsc_drv->connect_try > 1)
        {
            int i;

            printf("Connect to AP Failed:");
            for(i=0;i<6;i++)
                printf("%02x ",(unsigned char)wsc_drv->connect2BSSID[i]);
            for(i=0;i<wsc_drv->ssid_len;i++)
                printf("%c", (unsigned char)wsc_drv->ssid[i]);
            printf("\n");

            if ((wsc_drv->blockAPNum < 32) && (wsc_drv->policy == 0))
            {
                memcpy(wsc_drv->blockBssid[wsc_drv->blockAPNum], wsc_drv->connect2BSSID, ETH_ALEN);
                wsc_drv->blockTime[wsc_drv->blockAPNum] = wsc_drv->wpsAPNum;
                wsc_drv->blockAPNum++;
            }
            wsc_drv->connect_try = 0 ;
            wsc_new_state(wsc_drv, SM_SCAN);

            wsc_drv->ssid_len = 0;

#ifdef L2DEINIT
            if (wsc_drv->l2 != NULL)
            {
                l2_packet_deinit(wsc_drv->l2);
                wsc_drv->l2 = NULL;
            }

            wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                    ETH_P_ALL/*ETH_P_EAPOL*/, wsc_supplicant_rx, wsc_drv, 0);
#else
     	    wps_driver_zydas_wps_filter(wsc_drv,1);
#endif

            wpa_driver_wext_scan(wsc_drv);

            /* Set timeout */
            //wsc_set_timeout(wsc_drv, WSC_TIMEOUT_SCAN, wps_state_table[wsc_drv->state]);
            return;

        }
        wsc_drv->connect_try ++ ;

        wsc_supplicant_connect(wsc_drv);

        /* Set timeout */
        wsc_set_timeout(wsc_drv, 3/*WSC_TIMEOUT_DEFAULT*/, wps_state_table[wsc_drv->state]);
    }
    if (wsc_drv->state == SM_SCAN)
    {
        int policy0_pbcOverlap = 0;
        policy0_pbcOverlap = (wsc_drv->policy == 0) && (wsc_drv->pin == NULL) && (wsc_drv->pbcActivates > 1);
        if ((wsc_drv->pbcAPNum == 0) || (wsc_drv->pbcAPNum > 1) || (policy0_pbcOverlap))
        {
            if(policy0_pbcOverlap)
                printf("Find %d WPS APs\n", wsc_drv->pbcActivates);
            else
                printf("Find %d WPS APs\n", wsc_drv->pbcAPNum);
            wsc_drv->pbcAPNum = 0;
            
#ifdef L2DEINIT 
            if (wsc_drv->l2 != NULL)
            {
                l2_packet_deinit(wsc_drv->l2);
                wsc_drv->l2 = NULL;
            }
            wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                                    ETH_P_ALL/*ETH_P_EAPOL*/, wsc_supplicant_rx, wsc_drv, 0);
#else
     	    wps_driver_zydas_wps_filter(wsc_drv,1);

#endif
                                    
            wsc_decrease_and_remove_block_ap(wsc_drv);
            
            /* Cancel the connect timer */
            wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);
            
            wsc_drv->wpsAPNum = 0;
            wsc_drv->pbcActivates = 0;
            wpa_driver_wext_scan(wsc_drv);
            
            /* Set timeout , already set in wpa_driver_wext_scan */

            //wsc_set_timeout(wsc_drv, WSC_TIMEOUT_SCAN, wps_state_table[wsc_drv->state]);
        }
        else
        {
            wsc_decrease_and_remove_block_ap(wsc_drv);
#ifdef L2DEINIT
            if (wsc_drv->l2 != NULL)
            {
                l2_packet_deinit(wsc_drv->l2);
                wsc_drv->l2 = NULL;
            }
            
            wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                                    ETH_P_EAPOL, wsc_supplicant_rx, wsc_drv, 0);
#else
     	    wps_driver_zydas_wps_filter(wsc_drv,0);
#endif

            // Cancel timeout
            wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

            /* Initialize state machine */
			wsc_supplicant_deauthenticate(wsc_drv, wsc_drv->bssid, 3);

#if 0
            if (set80211param(wsc_drv, IEEE80211_PARAM_ROAMING, 2, 1) < 0) {
                wsc_printf(WSC_DBG_INIT, "%s: failed to set wpa_supplicant-based "
                        "roaming\n", __FUNCTION__);
            }

            if (set80211param(wsc_drv, IEEE80211_PARAM_WPA, 3, 1) < 0) {
                wsc_printf(WSC_DBG_INIT, "%s: failed to enable WPA support\n",
                        __FUNCTION__);
            }
#endif
            if((wsc_drv->policy == 0) && (wsc_drv->pin == NULL) && (wsc_drv->pbcActivates == 1))
            {
                memcpy(wsc_drv->ssidBody,wsc_drv->PBCssidBody,32);
                wsc_drv->ssid_len = wsc_drv->PBCssid_len;
                printf("1 PBC found in policy=0, try it directly\n");
            }

	    wsc_new_state(wsc_drv, SM_CONNECT);
            wsc_supplicant_timeout_handler(wsc_drv);


            //wsc_set_timeout(wsc_drv, WSC_TIMEOUT_INIT, wps_state_table[wsc_drv->state]);
            
            wsc_drv->pbcAPNum = 0;
        }
    }
    else if (wsc_drv->state == SM_START || wsc_drv->state == SM_SEND_IDENTITY
            || wsc_drv->state == SM_START_WPS_NEGOCIATION)
    {
        //wsc_new_state(wsc_drv, SM_INIT);
        
        if (wsc_drv->state == SM_SEND_IDENTITY)
        {
            if (0 && ++wsc_drv->resendTime < 3)
            {
                // Cancel timeout
                wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

                /* Resend a eap response frame */
                wpa_supplicant_eapol_send(wsc_drv, IEEE802_1X_TYPE_EAP_PACKET, (u8 *)rsp,
                              ntohs(rsp->length));

                wsc_set_timeout(wsc_drv, WSC_TIMEOUT_INIT, wps_state_table[wsc_drv->state]);

                return;
            }
        }
        
        
        if ((wsc_drv->blockAPNum < 32) && (wsc_drv->policy == 0))
        {
            memcpy(wsc_drv->blockBssid[wsc_drv->blockAPNum], wsc_drv->bssid, ETH_ALEN);
            //This number implictly means that STA can operate in where maximum number of WPS APs 
            wsc_drv->blockTime[wsc_drv->blockAPNum] = wsc_drv->wpsAPNum;

            wsc_drv->blockAPNum++;
        }
        wsc_new_state(wsc_drv, SM_SCAN);

        wsc_drv->ssid_len = 0;
        
        /* Initialize state machine */
        if (wsc_drv->pin == NULL)
            InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x04, NULL);
        else
            InitStateMachine(wsc_drv->l2->own_addr, wsc_drv->bssid, 0x0, wsc_drv->pin);

        if (memcmp(wsc_drv->bssid, "\x00\x00\x00\x00\x00\x00",
                ETH_ALEN) != 0)
        {
            wsc_supplicant_deauthenticate(wsc_drv, wsc_drv->bssid, 3);
        }

        /* Do STA connect */
        //wsc_supplicant_connect(wsc_drv);

#ifdef L2DEINIT
        if (wsc_drv->l2 != NULL)
        {
            l2_packet_deinit(wsc_drv->l2);
            wsc_drv->l2 = NULL;
        }
        
        wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                                ETH_P_ALL/*ETH_P_EAPOL*/, wsc_supplicant_rx, wsc_drv, 0);
#else
	wps_driver_zydas_wps_filter(wsc_drv,1);
#endif
                                
        wpa_driver_wext_scan(wsc_drv);

        /* Set timeout */
        //wsc_set_timeout(wsc_drv, WSC_TIMEOUT_SCAN, wps_state_table[wsc_drv->state]);
        
    }
}

int wsc_supplicant_run(struct wsc_drv *wsc_drv)
{
    extern int connect_start_time;
    fd_set rfds;
    int res;
    int flags;
    struct timeval tv, now, tmp;

    wsc_set_timeout(wsc_drv, WSC_TIMEOUT_INIT, wps_state_table[wsc_drv->state]);

    //if (wsc_drv->pin != NULL)
    if (wsc_drv->startOP != 0)
    {
		if (wsc_drv->startOP == 1)
		    wsc_printf(WSC_DBG_CONNECT, "PIN method chosen, PIN = %s, Start Scanning.\n", wsc_drv->pin);
		else if (wsc_drv->startOP == 2)
		    wsc_printf(WSC_DBG_CONNECT, "Push Button method chosen, Start Scanning.\n");
		/* kill wpa_supplicant process */
		if (wsc_drv->wpa_supplicant_pid != 0)
		{
            wsc_kill_process(wsc_drv->wpa_supplicant_pid);
            wsc_kill_process(wsc_drv->wpa_supplicant_pid + 2);
            system( "killall hostapd" ); /* kill hostapd because of wsccmd would not do it */
            sleep(3); /* wait 2 seconds, hostapd eloop bug */
            wsc_drv->wpa_supplicant_pid = 0;
        }

        #if 0
		/* kill hostapd process, wsccmd should do this */
        if (wsc_drv->hostapd_pid != 0)
		{
            wsc_kill_process(wsc_drv->hostapd_pid);
            wsc_drv->hostapd_pid = 0;
        }
        #endif

        if (wsc_drv->wds_ap_ifname != NULL)
        {
			/* kill wsccmd process */
            if (wsc_drv->wsccmd_pid != 0)
	    	{
                wsc_kill_process(wsc_drv->wsccmd_pid);
                wsc_drv->wsccmd_pid = 0;
            }
        }

        /* 
         * Moved to beginning of madwifi_init(). This brings up the
         * AP and STA vaps before any ioctl()s are set.
         */ 
#if 1
        /* Set the interface wds_ap up
        */
        if (wsc_drv->wds_ap_ifname != NULL)
        {
            if (wpa_driver_wext_get_ifflags(wsc_drv, wsc_drv->wds_ap_ifname, &flags) != 0 ||
                wpa_driver_wext_set_ifflags(wsc_drv, wsc_drv->wds_ap_ifname, flags | IFF_UP) != 0) {
                wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", wsc_drv->wds_ap_ifname);
            }
        }

        /* Set the interface wds_sta up
        */
        if (wpa_driver_wext_get_ifflags(wsc_drv, wsc_drv->ifname, &flags) != 0 ||
            wpa_driver_wext_set_ifflags(wsc_drv, wsc_drv->ifname, flags | IFF_UP) != 0) {
            wsc_printf(WSC_DBG_CONNECT, "Could not set interface %s UP\n", wsc_drv->ifname);
        }
#endif

#ifdef L2DEINIT
        if (wsc_drv->l2 != NULL)
        {
            l2_packet_deinit(wsc_drv->l2);
            wsc_drv->l2 = NULL;
        }
        
        wsc_drv->l2 = l2_packet_init(wsc_drv->ifname, wpa_drv_get_mac_addr(wsc_drv),
                                ETH_P_ALL/*ETH_P_EAPOL*/, wsc_supplicant_rx, wsc_drv, 0);
#else
	wps_driver_zydas_wps_filter(wsc_drv,1);
#endif
        
        /* Check whether we need to initialize bridge socket */
        if (wsc_drv->bridge_ifname != NULL)
        {
            wsc_printf(WSC_DBG_INIT, "Initialize bridge interface\n");
        
            if (wsc_drv->l2_br == NULL)
            {
                wsc_drv->l2_br = l2_packet_init(wsc_drv->bridge_ifname,
                                            NULL,
                                            ETH_P_EAPOL, wsc_supplicant_rx,
                                            wsc_drv, 0);
            }
        }

		/* Cancel timeout */
		wsc_supplicant_cancel_timeout(wsc_drv, wps_state_table[wsc_drv->state]);

		wsc_new_state(wsc_drv, SM_SCAN);

		memcpy(wsc_drv->bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN);
		wsc_printf(WSC_DBG_CONNECT, "Set AP address are all zeros\n");

        wsc_drv->ssid_len = 0;
        
        wsc_drv->pbcAPNum = 0;
        
        wsc_drv->pushButtonPress = 1;

        /* Set timeout */
        wsc_set_timeout(wsc_drv, 1, wps_state_table[wsc_drv->state]);//This scan is useless. Just use trigger the stateMachine
    }
    
    if (zydas_set_param(wsc_drv, ZD_PARAM_ROAMING, 1, 1) < 0) //Stop drv scanning
        printf("Set Roamming failed\n");
    wsc_supplicant_deauthenticate(wsc_drv, wsc_drv->bssid, 3);
    if (wpa_driver_wext_set_ssid(wsc_drv, "|||-_-|||", 9) < 0)
        printf("Set SSID Failed\n");
    sleep(2);

    start_time = time(NULL);
    while(1)
    {
        int ii;
        if((time(NULL) - start_time > 120) && (connect_start_time == 0))
        {
            printf("No available AP found within 120 seconds. Abort\n");
            break;
        }
        if((connect_start_time != 0) && (time(NULL) - connect_start_time > 90))
        {
            printf("60 seconds timeout to connect AP. Abort\n");
            break;
        }

        FD_ZERO(&rfds);
        wsc_copy_fd_table(wsc_drv, &rfds);
        gettimeofday(&tmp, NULL);

        if (wsc_drv->timeout) {
            gettimeofday(&now, NULL);
            if (timercmp(&now, &(wsc_drv->timeout->time), <))
                timersub(&(wsc_drv->timeout->time), &now, &tv);
            else
                 tv.tv_sec = tv.tv_usec = 0;
        }

        res = select(wsc_drv->max_sock_num+1, &rfds, NULL, NULL,
                  (wsc_drv->timeout) ? &tv : NULL);

        if (res < 0 && errno != EINTR) {
            perror("select");
            exit(0);
        }

        /* check if some registered timeouts have occurred */
        if (wsc_drv->timeout) {
            struct eloop_timeout *tmp;

            gettimeofday(&now, NULL);
            if (!timercmp(&now, &(wsc_drv->timeout->time), <)) {
                tmp = wsc_drv->timeout;
                wsc_drv->timeout = wsc_drv->timeout->next;
                wsc_supplicant_timeout_handler(wsc_drv);
                free(tmp);
            }

        }

        for (ii = 0; ii < wsc_drv->sock_num; ii++)
        {
            if (FD_ISSET(wsc_drv->event_sock, &rfds))
            {
                wpa_driver_wext_event_receive(wsc_drv);
            }
            else if (FD_ISSET(wsc_drv->sock[ii], &rfds))
            {
                wsc_printf(WSC_DBG_MSGWSC, "Receive UDP data: %d\n", wsc_drv->sock[ii]);
                l2_packet_receive(wsc_drv->sock[ii], wsc_drv->l2_sock[ii], NULL);                
            }
        }
    }
    return 0;
}

int wsc_supplicant_close_network(struct wsc_drv *drv)
{
    int flags;

    /* Set the network interface down */
    if (wpa_driver_wext_get_ifflags(drv, drv->ifname, &flags) != 0 ||
        wpa_driver_wext_set_ifflags(drv, drv->ifname, flags & ~IFF_UP) != 0)
    {

        wsc_printf(WSC_DBG_INIT, "Could not set interface %s DOWN\n", drv->ifname);
        return -1;
    }

    return 0;
}

int wsc_supplicant_wait_connection(struct wsc_drv *drv)
{
    char bssid[ETH_ALEN];

    if (wpa_driver_wext_get_bssid(drv, bssid) == 0 &&
        memcmp(bssid, "\x00\x00\x00\x00\x00\x00", ETH_ALEN) != 0)
    {
        memcpy(drv->bssid, bssid, ETH_ALEN);
        return 1;
    }

    return 0;
}

int wsc_supplicant_send_eapol_start(struct wsc_drv *drv)
{
    int ret;
    //wsc_printf(WSC_DBG_CONNECT, "CWY - Send eapol start packet\n");
    ret = wpa_supplicant_eapol_send(drv, IEEE802_1X_TYPE_EAPOL_START, NULL, 0);
    return ret;
}

#define simpleconfigAuthenticationType_Open    0x0001
#define simpleconfigAuthenticationType_WPAPSK  0x0002

int wsc_write_wpa_supplicant_conf(FILE *fp, int wHead,char *ssid, int key_mgmt, int enc, int keyIndex, char *nwKey, u32 nwkeylen)
{

#if 0
    FILE *fp = NULL;

    fp = fopen(SUPP_CONF_FILENAME, "w");

    if (fp == NULL)
        return -1;
#endif

    //fprintf(fp, "ctrl_interface=/var/run/wpa_supplicant\n");
    if (wHead)
    {
        fprintf(fp, "eapol_version=1\n");
        fprintf(fp, "ap_scan=1\n");
    }
    fprintf(fp, "network={\n");
    fprintf(fp, "\tssid=\"%s\"\n", ssid);

    if (key_mgmt == simpleconfigAuthenticationType_Open)
    {
        fprintf(fp, "\tkey_mgmt=%s\n", "NONE");
        if (enc == wlanProfileSecurityMode_eWep)
        {
            char line[256];

            fprintf(fp, "\twep_tx_keyidx=%d\n", keyIndex);
            switch (keyIndex)
            {
            case 0:sprintf(line, "\twep_key0=");
                   break; 
            case 1:sprintf(line, "\twep_key1=");
                   break; 
            case 2:sprintf(line, "\twep_key2=");
                   break; 
            case 3:sprintf(line, "\twep_key3=");
                   break; 
            }
            if ((nwkeylen == 5) || (nwkeylen == 13))
            {
                strcat(line, "\"");
                strcat(line, nwKey);
                strcat(line, "\"");
            }
            else if ((nwkeylen == 10) || (nwkeylen == 26))
            {
                strncat(line, nwKey, nwkeylen);
            }
            fprintf(fp, "%s\n", line);
        }
    }
    else
    {
        fprintf(fp, "\tkey_mgmt=%s\n", "WPA-PSK");

        // Assuming non-zero nwKeyLen
        if (nwkeylen > 0)
        {
            char line[256];
            
            /*
             * Don't specify proto or pairwise.
             * Leave them at default.
             */ 
#if 0       
            if (key_mgmt == 0x0002)
                fprintf(fp, "\tproto=WPA\n");
            else if (key_mgmt == 0x0020)
                fprintf(fp, "\tproto=RSN\n");
            fprintf(fp, "\tpairwise=TKIP\n");
#endif      
            sprintf(line, "\tpsk=");
            
            if (nwkeylen < 64)
            {
                // Interpret as passphrase
                // Must be 8-63 characters
                strcat(line, "\"");
                strcat(line, nwKey);
                strcat(line, "\"");
            }
            else
            {
                // Interpret as PSK
                // Must be exactly 64 characters
                strncat(line, nwKey, 64);
            }
            
            fprintf(fp, "%s\n", line);
        }
    }
    fprintf(fp, "\tscan_ssid=1\n");
    fprintf(fp, "}\n");

#if 0
    fclose(fp);
#endif
    return 0;
}

int wsc_write_wsccmd_conf(char *ssid, int key_mgmt, int enc,
        int keyIndex, char *nwKey, u32 nwkeylen)
{
#define WSCCMD_CONF_FILENAME    "/etc/ath/wsc_config.txt"

    FILE *fp = NULL;
    int ii;

    fp = fopen(WSCCMD_CONF_FILENAME, "w");

    if (fp == NULL)
    {
        wsc_printf(WSC_DBG_PROFILE, "Can't open configuration file: %s\n",
            WSCCMD_CONF_FILENAME);
        return -1;
    }

    for (ii = 0; ii < sizeof(wsc_config)/sizeof(wsc_config[0]); ii++)
    {
        if (wsc_config[ii][0] == '#')
        {
            fprintf(fp, wsc_config[ii]);
        }
        else
        {
            if (strstr(wsc_config[ii], "SSID"))
            {
                fprintf(fp, "%s%s\n", wsc_config[ii], ssid);
            }
            else if (strstr(wsc_config[ii], "KEY_MGMT"))
            {
                /*
                 * Keep it at WPA-PSK for now.
                 */
                //char *keymgmt = (key_mgmt == 0x0002) ? "WPA-PSK" : "WPA2-PSK";
                char *keymgmt = (key_mgmt == 0x0002) ? "WPA-PSK" : "OPEN";
                //char *keymgmt = "WPA-PSK";

                fprintf(fp, "%s%s\n", wsc_config[ii], keymgmt);
            }
            else if (strstr(wsc_config[ii], "AUTH_TYPE_FLAGS"))
            {
                fprintf(fp, "%s%d\n", wsc_config[ii], key_mgmt);
            }
            else if (strstr(wsc_config[ii], "ENCR_TYPE_FLAGS"))
            {
                fprintf(fp, "%s%d\n", wsc_config[ii], enc);
            }
            else if (strstr(wsc_config[ii], "NW_KEY"))
            {
                char line[256] = { 0 };

                if (nwkeylen < 64)
                {
                    // Interpret as passphrase
                    // Must be 8-63 characters
                    strcat(line, "\"");
                    strcat(line, nwKey);
                    strcat(line, "\"");
                }
                else
                {
                    // Interpret as PSK
                    // Must be exactly 64 characters
                    //strncat(line, nwKey, 64);
                    memcpy(line, nwKey, 64);
                    line[64] = '\0';
                }

                fprintf(fp, wsc_config[ii]);
                fprintf(fp, line);
                fprintf(fp, "\n");
                //fprintf(fp, "%s%s\n", wsc_config[ii], line);
            }
            else
            {
                fprintf(fp, wsc_config[ii]);
            }
        }
    }

    fclose(fp);
    return 0;
}

int wsc_write_hostapd_conf(char *ssid, int key_mgmt,
        char *nwKey, u32 nwkeylen)
{
#define HOSTAPD_CONF_FILENAME    "/etc/ath/hostapd.conf"

    FILE *fp = NULL;
    int ii;

    fp = fopen(HOSTAPD_CONF_FILENAME, "w");

    if (fp == NULL)
    {
        wsc_printf(WSC_DBG_PROFILE, "Can't open configuration file: %s\n",
            HOSTAPD_CONF_FILENAME);
        return -1;
    }

    for (ii = 0; ii < sizeof(hostapd_conf)/sizeof(hostapd_conf[0]); ii++)
    {
        if (hostapd_conf[ii][0] == '#')
        {
            fprintf(fp, hostapd_conf[ii]);
        }
        else
        {
            if (strstr(hostapd_conf[ii], "ssid"))
            {
                fprintf(fp, "%s%s\n", hostapd_conf[ii], ssid);
            }
            else if (strstr(hostapd_conf[ii], "wpa_key_mgmt"))
            {
                char *keymgmt = "WPA-PSK";

                fprintf(fp, "%s%s\n", hostapd_conf[ii], keymgmt);
            }
            else if (strstr(hostapd_conf[ii], "wpa_psk"))
            {
                char line[256];

                if (nwkeylen < 64)
                {
                    // Interpret as passphrase
                    // Must be 8-63 characters
                    strcat(line, "\"");
                    strcat(line, nwKey);
                    strcat(line, "\"");
                }
                else
                {
                    // Interpret as PSK
                    // Must be exactly 64 characters
                    //strncat(line, nwKey, 64);
                    memcpy(line, nwKey, 64);
                    line[64] = '\0';
                }

                fprintf(fp, hostapd_conf[ii]);
                fprintf(fp, "%s\n", line);
                //fprintf(fp, "%s%s\n", hostapd_conf[ii], line);
            }
            else
            {
                fprintf(fp, hostapd_conf[ii]);
            }
        }
    }

    fclose(fp);
    return 0;
}

pid_t wsc_invoke_wpa_supplicant(char *ifname, char *path, char *config)
{
#if 0
    char buf[128];

    sprintf(buf, "/sbin/wpa_supplicant -Dmadwifi -i%s -c %s/%s -b br0", ifname, path, config);
    system(buf);
#else
    pid_t pid;
    char arg_ifname[20];
    char arg_config[80];

    //sprintf(buf, "-Dmadwifi -i%s -c %s/%s -b br0", ifname, path, config);

    sprintf(arg_ifname, "-i%s", ifname);
    //sprintf(arg_config, "%s/%s", path, config); 
    sprintf(arg_config, "%s", config); 

    pid = fork();

    /* Child process */
    if (pid == 0)
    {
        execl("/usr/bin/wpa_supplicant", "wpa_supplicant", "-Dzydas",
                arg_ifname, "-c", arg_config, 0);
        //execlp("wpa_supplicant", "-Dzydas",
        //        arg_ifname, "-c", arg_config, 0);
    }
#endif

    return pid;
}

pid_t wsc_invoke_wsccmd(char *path, int radio_num)
{
#if 0
    char buf[128];

    sprintf(buf, "/sbin/wsccmd -B %s %d", path, radio_num);
    system(buf);
#else
    pid_t pid;
    char arg_path[80];
    char arg_radio[10];
    //char buf[128];

    //sprintf(buf, "-B %s %d", path, radio_num);
    sprintf(arg_path, "%s", path);
    sprintf(arg_radio, "%d", radio_num);

    pid = fork();

    /* Child process */
    if (pid == 0)
    {
        execl("/sbin/wsccmd", "wsccmd", "-B", arg_path, arg_radio, 0);
    }
#endif

    return pid;
}

#if 0
pid_t wsc_invoke_hostapd(char *path, char *config)
{
#if 0
    char buf[128];

    sprintf(buf, "/sbin/hostapd -B %s/%s", path, config);
    system(buf);
#else
    pid_t pid;
    char arg_config[80];
    //char buf[128];

    //sprintf(buf, "-B %s/%s", path, config);

    sprintf(arg_config, "%s/%s", path, config);

    pid = fork();

    /* Child process */
    if (pid == 0)
    {
        execl("/sbin/hostapd", "hostapd", arg_config, 0);
    }
#endif

    return pid;
}
#endif

void wsc_supplicant_destroy(struct wsc_drv *drv)
{
    /* Close socket */
    if (drv->l2 != NULL)
    {
        close(drv->l2->fd);
        free(drv->l2);
    }

    if (drv->l2_br != NULL)
    {
        close(drv->l2_br->fd);
        free(drv->l2_br);
    }

    wsc_supplicant_close_network(drv);
    free(drv);
}

